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ABSTRACT 


To  keep  the  operational  cost  down  and  increase  the  mission  time  with 
minimum  human  intervention,  autonomous  recovery  or  docking  operation  of  an 
Autonomous  Underwater  Vehicle  (AUV)  is  required.  Central  to  the  successful 
autonomous  docking  process  of  the  AUV  is  the  capability  of  the  AUV  being  able 
to  track  and  steer  itself  accurately  towards  the  dock  which  is  constantly  perturbed 
by  wave  motion  effects.  In  addition,  for  accurate  acoustic  homing  during  the  final 
stages  of  the  docking,  the  AUV  requires  acoustic  systems  with  high  update  rates. 
Equipped  with  acoustic  modem,  ARIES  had  experimentally  been  tested  to  have 
an  update  rate  of  only  about  0.3  Hz.  These  delayed  data  can  potentially  cause  a 
false  commanded  reference  input  to  the  tracking  system  in  between  the  updates 
and  cause  ARIES  to  miss  the  moving  cage’s  entrance. 

This  thesis  attempts  to  investigate  the  effectiveness  on  the  use  of  cross 
track  error  and  line  of  sight  error  sliding  mode  controller  coupled  with  dynamic 
waypoints  allocation  in  horizontal  steering  of  ARIES  in  docking  operations.  In  the 
absence  of  cage  heading  updates,  a  predictive  method  based  on  angular  rate 
and  direction  of  motion  was  used  to  estimate  the  dynamics  of  the  moving  cage. 
Further  analysis  was  performed  in  order  to  understand  the  limitations  of  such  an 
implementation. 
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I.  INTRODUCTION 


A.  BACKGROUND 

A  docking  system  is  a  crucial  component  in  an  Autonomous  Underwater 
Vehicle  (AUV)  system  since  it  greatly  enhances  AUV  mission  time  and  reduces 
its  operation  cost.  It’s  primary  purpose  is  to  perform  the  tasks  of  deploying  and 
recovering,  downloading  mission  data,  uploading  new  instructions  to  the  vehicle, 
real-time  vehicle  diagnostics  and  recharging  of  vehicle  batteries.  With  these 
capabilities,  the  docking  system  can  greatly  increase  the  range  of  potential 
applications  possible  with  an  AUV  and  further  enhance  its  autonomy  and 
operation  time  in  its  missions.  It  is  also  known  that  a  large  part  of  operational 
cost  of  AUV  is  attributed  to  the  ship  and  personnel  time  required  to  get  the  AUV 
on  station  to  begin  a  deployment,  and  the  AUV  recovery  time  at  the  end  of  a 
mission.  By  automating  the  docking  process,  these  overheads  can  be  greatly 
reduced  making  the  AUV  operationally  more  viable. 

Central  to  the  successful  docking  process  of  the  AUV  is  the  capability  of 
the  vehicle  to  track  and  steer  itself  accurately  towards  the  dock  that  is  constantly 
perturbed  by  wave  motion  effects.  The  type  of  docking  system  employed  also 
dictates  the  complexity  of  the  docking  process.  These  can  be  broadly  classified 
into  two  categories  [1]: 

•  Those  that  equipped  the  AUV  with  special  mounting  hardware  to 
grab  a  vertical  pole  or  line 

•  Tethered  cage  with  protective  housing  and  it  requires  the  AUV  to 
swim  in 

The  latter  approach  is  the  area  of  interest  in  this  thesis.  It  presents  a  more 
challenging  navigation  problem  than  the  former,  as  the  AUV  must  orient  itself  to 
be  aligned  with  the  cage  entrance.  The  cage  platform  that  is  tethered  to  a  ship  at 
some  depth  below  the  surface  of  the  water  has  its  dynamics  greatly  influenced  by 
wave  motion  effects  of  the  ship  and  ocean  currents.  These  perturbations  to  the 
cage  heading  and  position  may  easily  move  it  away  from  the  approaching  path  of 
the  AUV.  In  order  for  the  AUV  to  guide  itself  to  the  entrance  of  the  cage,  some  a 
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priori  knowledge  of  which  way  the  cage  is  facing  is  required.  This  requires  some 
form  of  a  heading  measuring  device  such  as  an  electronic  compass  on  the  dock 
and  an  acoustic  modem  that  can  transmit  the  information  to  the  AUV  with  the 
shortest  possible  delay.  With  the  cage  orientation  known,  the  AUV  will  need  to 
have  the  capability  to  compensate  for  the  cage’s  motion  when  making  its  final 
approach  to  the  cage  in  order  for  it  to  dock  successfully. 


B.  SCOPE  OF  THIS  WORK 

The  docking  problem  can  be  decoupled  and  linearized  into  a  set  of 
dynamics  problems  in  the  vertical  and  horizontal  plane.  The  main  area  of  interest 
in  this  study  is  in  steering  control  of  AUV  to  a  swinging  cage  in  the  x-y  plane.  The 
NPS  AUV  platform,  ARIES  is  used  for  the  study. 

The  focus  of  this  thesis  is  two-fold: 

1 .  Develop  a  set  of  trajectory  paths  for  approaching  the  docking  cage 
and  investigate  its  performance  under  fix  and  moving  cage’s  headings. 

2.  Investigate  the  impact  of  time  delay  in  data  update  caused  by  the 
acoustic  modem  and  further  design  a  method  to  account  for  this  time  delay. 

Chapter  II  will  focus  on  the  equations  of  motion  for  ARIES  AUV  and  the 
associated  steering  control  laws.  Chapter  III  will  discuss  trajectory  design  for 
docking  the  AUV  and  its  implementation.  Chapter  IV  will  present  the  simulation 
results  and  their  limitations.  Chapter  V  will  discuss  the  theory  and  design  of  an 
estimator  that  can  observe  the  extra  state  of  the  cage  heading  and  present  the 
simulation  results.  Finally,  Chapter  VI  will  offer  some  conclusions  and 
recommendations  for  future  studies. 
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II.  EQUATIONS  OF  MOTION  AND  AUV  MODELING 


A.  SYSTEM  OF  EQUATIONS  OF  RIGID  BODY  MOTION 

This  section  presents  a  brief  description  of  derivation  of  equations  of 
motion  for  underwater  rigid  body  which  is  essential  for  AUV  modeling.  For  a  start, 
the  two  different  coordinate  systems,  global  and  body  reference  frame  and  their 
relationship  are  defined  in  Figure  1 . 


Coordinate  System  and  Positional  Definitions 

According  to  Flealy  [2],  the  global  navigation  frame  OXYZ  assumes  a  NED 
coordinate  convention  and  by  ignoring  the  earth’s  rotational  rate,  it  is  also  the 
“inertial”  reference  frame  in  which  Newton’s  Laws  of  Motion  will  be  valid.  The 
vehicle  body  fixed  reference  frame  is  defined  with  Y  axis  pointing  to  the  right,  X 
axis  along  the  longitudinal  axis  of  the  vehicle  and  the  Z  axis  is  positive 
downwards.  The  origin  of  the  reference  frame  lies  at  the  vehicle’s  center  which  is 
different  from  the  vehicle’s  center  of  mass  (G). 

It  follows  that  the  vehicle  position  with  respect  to  the  global  origin,  Ro  is 
defined  asR^=[Xf^I +  YJ +  Z^K]  or  in  vector  form,  R^=[Xq,Y^,Z^].  Thus  the 
global  velocity 
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(1) 


u 

w 


Where: 


cos  cos  ^  sin  cos  ^  -sin^ 

T{^,0,i/r)=  cosi/^sm0sm^-smifrcos^  sin  sin  ^  sin  + cos  cos  cos^sin^^ 
cos^^sin0cos(^  +  sin^^sin(^  sin^^sin^cos^^-cos^^sin^^  cos^cos^^ 


u,  V,  w  =  Vehicle’s  forward  speed  (surge),  side  slip  velocity  (sway)  and 
component  velocities  in  local  Z  direction  (heave)  respectively. 

(j),  0 ,  y/  =  Euler  angles 

The  rate  of  change  of  Euler  angles  can  in  turn  be  found  through  the  following 
relationship: 

^  1  sin  tan  0  cos  tan  0  p 

0  =  Q  cos^  -sin^^  q  (2) 

ip  Q  sin  cos  <!>  r 

COS^  COS0 

Where: 

p,  q,  r  =  body  fixed  components  of  the  angular  velocity  vector. 

The  inertial  frame  position  of  the  vehicle’s  center  of  mass  can  be  further 
expressed  as .  Since  the  center  of  mass  lies  in  a  body  that  is 
translating  and  rotating,  the  total  time  derivative  of  Rq  becomes 

dR 

—f^  =  R^  +  0)xp^=v  +  0)xp^  (3) 

at 

Where: 

CO  =  [p,q,r] 

Pg  “[^G’Tg’-^g] 
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The  translational  Equation  of  Motion  can  then  be  expressed  as 


F  =  m - ^ 

de  (4) 

=  m{v  +  6)x  p^  +  0)xo)x  pQ+0)Xv) 

and  the  rotational  Equation  of  Motion  as 

dFl  (  d^ R  ^ 

M^= — -+ p^x  m — orM^=  I^d)+CDx{l^cD)  +  m{p^xv  + p^xcDXv)  {5) 
dt  dt 


Where; 


Angular  momentum  of  body,  Ho=IoCO 


=  lQ)+COXH„ 


Mass  moment  of  inertia, 


^ XX  ^ xy  ^ xz 

^ yx  ^  yy  ^  yz 

^  zx  ^ zy  ^ zz 


^ dm.  (y^  +  dm,  {xy)  dm,  (xz) 

i-\  i-l  i-l 

N  N  N 

-^dm,{xy)  +  (6) 

i-l  i-l 

N  N  N 

-^dm,  (xz)  -S  dm,{yz)  ^dm,(x^+y^)\ 


To  incorporating  the  weight  and  buoyancy  forces  into  the  equation  of  motion,  the 
force  and  moment  vector  written  in  body  coordinates  as 

-sin0 

[W-B]  cos^sin^^ 

f  1  COS^COS(^ 

'  =  r  n  r  n 

-sin6'  -sin6> 

WPqX  cos^sin^^  -Bp^x  cos^sin^^ 
cos  0  cos  cos  0  cos 


can  be  added  negatively  to  the  right  hand  side  of  equation  (4)  and  (5)  resulting  in 
equation  (8)  and  (9)  which  form  the  six  degrees  of  freedom  equations  of  motion. 
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m 


dv 

—  +  coxPg 
\dt  j 


+  m[o)xo)x  Pg+0)xv)- 


Yf 


(8) 


I^d)+cox[l^co)  +  m[pGXv  +  pGXCoxv)-m^ 


Kf 

Mf 


(9) 


Further  modifications  can  be  done  to  account  for  ocean  current  by  substituting 
the  state  vector  of  [u  v  w  p  q  r]  with  relative  velocities  of 


[ur  K  P  d  r]  and  equation  (1)  will  be  modified  to 


Ur 

~U^~ 

Y 

+ 

Ury 

Z 

Uez_ 

(10) 


Where; 


^cx  >  ^cy ,  ^cz  ~  Ocean  current  velocities 

and  the  equations  of  six  degree  of  freedom  motion  will  be  expressed  as: 

SURGE  EQUATION  OF  MOTION 

m[u^-v^r  +  w^q-XG(q^  +  r^)  +  yG{pq-r)  +  ZG{pr  +  q)] 

+  {W-B)^me  =  X^  (11) 

SWAY  EQUATION  OF  MOTION 

m[v^+u/-w^p  +  XG{pq  +  r)-yG(p^  +  r^)  +  ZG{qr-p)] 

-[W  -  B)co?,9^m(l)  =  Yj.  (12) 
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in 


HEAVE  EQUATION  OF  MOTION 


m[w^-u^q  +  v^p  +  XQ[pr-q)  +  y^[qr  +  p)-ZQ[p^  +q^)] 

+  {W -B)cosOcos(I>  =  Z^  (13) 

ROLL  EQUATION  OF  MOTION 

hP  +  [l.-Iy)(ir  +  I,y{pr-q)-IyX(l' -r^]-I.Ap(l  +  r)  +  m[y^{w-u^q  +  v^p) 
-Zq  ( +  u^r  -  w^pA  -  (y^W  - yjjB)  cos 6 cos p  +  ( z^W  -  z^B)  cos 0 sin 

(14) 


PITCH  EQUATION  OF  MOTION 

lyA  +  (4 -I^pr- I,y  {qr  +  p)  +  ly,  {pq-r)  +  4  {w-u^q  +  v^p) 

-Zq  [u^  -v^r  +  w^qA  +  {xQW -XgB)cos0cosp  +  {zQW -z^B)sin0  =  Mj.  (15) 

YAW  EQUATION  OF  MOTION 

ij+(iy-iApq-ixy(p^-qA~^yApr+q)+hAqr-p)+m[xc{v^+u^r-w^p) 
-Pg  (“r  - +  w,q)]- {xqW - XgB)cos 0smp- {y^W - 7^5) sin 0  =  Nf  (16) 

Where: 

u^,  v^,  =  Component  velocities  for  a  body  fixed  system  with  respect  to 

the  water 

q,  r  =  Component  angular  velocities  for  a  body  fixed  system 
=  Weight 
=  Buoyancy 

=  Mass  moment  of  inertia  terms 

Xg,  yg,  Zg  =  Position  difference  between  geometric  center  of  AUV  and 
center  of  buoyancy 

Xg ,  jg ,  Zg  =  Position  difference  between  geometric  center  of  AUV  and 
center  of  gravity 

X^,  Yf,  Z^,  Kj,  M N j  =  Sums  of  all  external  forces  acting  on  AUV  in  the 
particular  body  fixed  direction 
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B.  STEERING  SYSTEM  MODEL 


By  assuming  symmetry  of  the  AUV,  we  may  simplify  the  six  degree  of 
freedom  motion  equations  to  horizontal  plane  maneuvering  of  the  AUV  and 
diving  behavior  in  the  vertical  plane  where  the  sway,  yaw  and  roll  are  coupled  but 
independent  of  heave,  pitch  and  surge  motions.  Since  the  area  of  interest  in  this 
study  is  in  steering  control  of  AUV,  the  steering  system  model  will  be  discussed 
in  more  details  in  this  section. 

Making  further  assumption  that  the  center  of  mass  of  the  vehicle  lies 
below  the  origin  (z^  is  positive)  while  and  are  zero,  motions  in  the  vertical 

are  negligible  (i.e.  [w^,  p,  q,  r,  Z,  (j),  0]  =  0),  and  equals  the  forward  speed, 
U.,  equations  11  through  16  can  be  simplified  as 


II 

(17) 

mv^  =  -mUy  +  AYj.  (r) 

(18) 

I J  =  AN  fit) 

(19) 

y/  =  r 

(20) 

X  =  U  ^cosy/  -vymy/ +U 

(21) 

Y  =  U ymif/  -v^co?,if/  +  U ^ 

(22) 

Through  the  assumption  of  ‘small’  motions,  fluid  forces  can  be  linearized 
using  Taylor  series  expansion.  The  expression  for  the  transverse  and  rotational 
force  can  then  be  expressed  as  in  equation  (23)  and  (24). 

+  +  +  (23) 

Nf  =  +  N^y^  +  Ny  +  Ny  (24) 

Where: 
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Y,. 


dY^ 


(Added  mass  in  sway  coefficient) 


3Y 

Y.  =  — -  (Added  mass  in  yaw  coefficient) 

3f 

dY 

Yv  (Coefficient  of  sway  force  induced  by  side  slip) 

'  ov. 


Y 


N,. 


aYf 

3r 


(Coefficient  of  sway  force  induced  by  yaw) 


aNf 

av. 


(Added  mass  moment  of  inertia  in  sway  coefficient) 


aN 

Nj  =  —-^  (Added  mass  moment  of  inertia  in  yaw  coefficient) 

af 


N., 


N. 


_aNf 

av^ 

aNf 

ar 


(Coefficient  of  sway  moment  from  side  slip) 


(Coefficient  of  sway  moment  from  yaw) 


In  addition,  the  action  of  the  rudder  will  produce  forces  that  when  linearized  are 
Yg5^  (t)  and  Ng5^  (t) .  The  dynamics  of  the  vehicle  are  thus  defined  as: 


mv,  =  -mUj  +  7,  +  7,  +  Y/  +  Y^r  +  Y^S^  (r) 

(25) 

Ij  =  +  N,r  +  N^r  +  NgS^  (t) 

(26) 

y/  =  r 

(27) 

Expressed  in  matrix  form  yields 


1 

S 

1 

-7, 

o' 

I 

Y^-mU^ 

o' 

I 

I _ _ 

0 

r 

— 

K 

0 

r 

+ 

Ns 

0 

0 

1 

0 

1 

0 

0 

(28) 


Johnson  [8]  further  simplified  the  mass  matrix  by  letting  the  cross  coupling 
terms  equal  to  zero  resulting  in  a  final  vehicle  dynamics  define  as: 
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0  olpj  o|vi  rr/ 

0  4-iV,  0  r  =  4  4  0  r  +  Ns  S^t)  (29) 

0  0  1  0  1  0  LH  L  0  _ 

and  determined  the  hydrodynamic  coefficients  and  mass  properties  for  ARIES  as 
follows: 


m-Y,^ 

m 

Ys 

Ns 

N. 

456.76 

222.26 

215 

69.90 

-68.16 

406.30 

-35.47 

-10.89 

-88.34 

Table  1.  Calculated  Hydrodynamic  Steering  Coefficients  and  mass  properties  of 

the  ARIES  AUV 


Resulting  in  the  following  forward  velocity  dependent  matrix  form: 

"456.76  0  OlTvl  [-68.16  406.3 -(222.26)4  [  69.9" 

0  215  0  r  =  -10.89  -88.34  0  r  +  -35.47  ^,(0(30) 

0  0  1  0  1  0  0 

C.  ARIES  CONTROL  LAWS  FOR  STEERING  AND  CROSS  TRACK 
ERROR 

In  order  to  design  a  trajectory  path  for  the  ARIES  AUV  to  steer  towards 
the  docking  cage,  control  laws  in  the  horizontal  plane  would  need  to  be  in  placed. 
There  is  existing  steering  and  cross  track  error  controller  design  for  ARIES  based 
on  sliding  mode  control  theory  presented  by  Healey  and  Lienard  (1993)  which 
can  help  to  jump-start  the  trajectory  design  process.  Using  these  existing 
controllers,  the  new  trajectory  path  design  will  be  presented  in  the  next  chapter 
and  further  investigation  on  compatibility  and  limitations  of  these  control  laws  with 
the  designed  trajectory  path  will  be  studied. 

This  section  describes  the  existing  control  laws  used  by  ARIES  for 
steering  and  determining  the  cross  track  error  which  provides  the  necessary 
background  knowledge  for  subsequent  work  in  this  thesis. 

Marco  and  Healey  [3]  proposed  the  use  of  a  combination  of  line  of  sight 
guidance  and  a  cross  track  error  (CTE)  control.  The  cross  track  error  control 
allows  for  ARIES  to  follow  a  straight-line  path.  However,  as  the  cross  track  error 
control  cannot  guarantee  stability  for  large  heading  errors  as  can  be  seen  in 
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Figure  3,  while  a  line  of  sight  heading  control  will  reduce  heading  errors  to  zero, 
alternating  between  the  two  controllers  will  minimize  both  cross  track  and 
heading  errors.  Figure  2  shows  the  track  geometry  and  velocity  vector  diagrams 
used  in  the  design  of  the  controllers. 


Track  Geometry  and  Velocity  Vector  (From;  Marco  and  Healey  [3]) 


Comparison  of  Cross  Track  Error  &  LOS  Error  Controller  versus  Cross  Track 

Error  Controller  alone 
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1.  Cross  Track  Error  (CTE)  Controller 

Cross  track  error,  8(t)  is  defined  as  the  perpendicular  distance  between 
the  center  of  the  vehicle  (X(t),y(t))  and  the  adjacent  track  line.  The  objective  of 
the  cross  track  error  controller  is  to  minimize  8(t)  so  that  the  vehicle  will  follow  the 
pre-planned  path  as  closely  as  possible.  With  CTE  control  in  place,  ARIES  can 
be  guided  to  follow  pre-planned  trajectory  using  waypoints  navigation. 

The  track  angle  is  defined  as: 

y^trk(i)  ~  Cretan  ~  ^wpt(i-\))  ) 

Where: 

( ^  )  and  ( )  are  the  current  and  previous  way 

points  respectively. 

The  cross  track  heading  error  for  the  /'^segment  is  then  defined  as: 

Wi^)cTE(i)  ~  ~  Wtrk{i)  (32) 

Where: 

W{t)cTE(i)  must  be  normalized  to  lie  between  ±180  degrees. 

The  difference  between  the  current  vehicle  position  and  the  next  waypoint  is: 


1 

II 

(33) 

1 

II 

(34) 

With  the  above  definitions,  the  distance  to  the  way  point  projected  to  the  track 
line  can  be  defined  as: 

^(^)i  ~[^wpt(i)  ^wpt(i)\*\-^^wpt(i)~  ^wpt(i-\))  ^'^wpt(i)~'^wpt{i-\))\^  (35) 

where  L.  is  the  total  track  length  is  defined  as  the  distance  between  the  and 
(/-l)^^  waypoints  and  is  given  by: 
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and  S{t).  ranges  between  0-100  percent  of  I,. 

The  cross  track  error  may  now  be  defined  as: 

£-(0  =  ‘S(0,sin(t/^(0)  (37) 

where  (r)  is  the  angle  (normalized  to  lie  between  ±180  degrees)  between  the 
line  of  sight  to  the  next  way  point  and  the  current  track  line  given  by: 

dp  (0  =  arctan  )  -  arctan  2(7 1  (0„p,(o  ) 

(38) 

Marco  and  Healey  [3]  continue  by  defining  the  sliding  surface  in  terms  of 
derivatives  of  the  cross  track  error  such  that  the  sliding  surface  for  the  CTE 
controller  becomes  a  second  order  polynomial  of  the  form: 

(j{t)  =  Ur{t)cos{\j/{t)„E(i))  +  \U  sm{y/{t)cTE(t))  +  (39) 

The  rudder  input  is  thus  expressed  as: 

s^t)  =  (t7Z)cos(^^(Oc7-ir(,))"‘(-^«''(Ocos(^^(OCT£(o)  +  ^(''(0)"  sin(^^(OCT£(o) 

-X,Ur{t)cos{y/{t)cTE(i))-^U  sm{y/{t)cTE(i))-'n{<^{t)l(l>)  (40) 

Where:  0  <  ^^(Ocnr(o  <  ~  ^  =0.6,  /I,  =0.1,  7  =0.1  and  (p  =0.5. 

71 

When  Wif)cTE(i)>~’  ARIES  will  still  follow  the  track  but  travel  in  the 

opposite  direction  to  that  desired.  In  order  to  prevent  this  from  happening  in 
practice,  a  bound  of  40  degrees  is  used  as  a  switch  to  line  of  sight  (LOS)  control. 

2.  Line  Of  Sight  Error  Controiier 

According  to  Marco  and  Healey  [3],  the  second  order  heading  control 
model  is: 
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r{t)  =  ar(t)  -1-  bS^  (t)  -1-  {disturbances) 

(41) 

Wit)  =  r{t) 

(42) 

Where: 

a  =  -0.30  sec  '  (determined  from  past  in-water  experiments) 
d  = -0.1125  sec  ^  (determined  from  past  in-water  experiments) 

{t)  =  Stern  rudder  angle. 

The  sliding  surface  and  stern  rudder  command  for  heading  control  is  thus  defined 
as: 

cr(0  =  -0.9499r(0  +  0.1701(^^_  -^^(0)  (43) 

(t)  =  -1 .543(2.5394r(0  +  rj  tanh((7(0/^))  (44) 

Where:  rj  =  (j)  =  0.5  and  Wcom~¥{f)  'S  the  heading  error. 

The  heading  command  and  LOS  error  could  be  determined  from: 

¥i^)com(LOS)  =  arctan  2(7  ,  X  (0„^,(,) )  (45) 

Wit), os  =  ¥it)comiLos)  -  ¥it)  (46) 

The  need  for  the  LOS  controller  is  apparent  in  two  cases:  1)  when  the 
mission  starts  and  ARIES’  initial  heading  is  greater  than  40  degrees  from  the 
initial  way  point  and  2)  when  the  angle  between  two  sequential  track  lines 
exceeds  40  degrees.  Once  the  cross  track  heading  error  reduces  to  less  than  40 
degrees,  ARIES  utilizes  the  CTE  controller. 
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III.  TRAJECTORY  DESIGN  FOR  DOCKING  ARIES 


A.  INTRODUCTION  AND  BACKGROUND 

As  introduced  in  Chapter  1,  the  challenge  in  docking  ARIES  into  the 
docking  cage  is  the  ability  of  ARIES  to  navigate  and  orient  itself  to  align  with  the 
cage  entrance  accurately  as  it  approaches  the  cage.  The  task  becomes  more 
daunting  when  the  tethered  cage  is  constantly  perturbed  by  ocean  current 
creating  oscillatory  swinging  motions  in  the  horizontal  plane  at  different  rates 
which  is  difficult  to  predict  and  thus  model  the  cage  dynamics. 

One  way  to  overcome  this  unknown  is  to  have  some  angle  and  position 
measuring  devices  on-board  the  docking  cage  and  an  acoustic  modem  which 
can  transmit  the  information  to  ARIES  with  the  shortest  possible  delay.  In 
experimental  work  conducted  by  Marr  [6],  two  commercially  available  modem 
systems  were  installed  in  the  ARIES  AUV  and  the  acoustic  transmission 
performance  of  each  was  thoroughly  investigated.  According  to  Marr  [6],  the 
minimum  one-way  acoustic  transmission  time  for  tactical  control  of  the  ARIES, 
neglecting  the  channel  delay  due  to  sound  velocity  is  three  seconds.  This 
minimum  three  seconds  update  rate  of  cage  position  and  heading  could 
potentially  cause  ARIES  to  miss  the  cage  entrance.  This  is  especially  so  for  fast 
moving  cage  dynamics. 

With  the  3  seconds  update  rate  constraint  in  mind,  trajectory  for  steering 
ARIES  into  the  cage  entrance  is  designed  using  waypoints  navigation  building  on 
top  of  existing  cross  track  error  and  line  of  sight  error  controller. 

The  approach  taken  for  the  design  process  was  first  to  determine  the 
conditions  which  qualify  for  successful  docking.  The  number  of  waypoints  and 
waypoints  location  were  then  automatically  determined  based  on  the  starting 
point  of  ARIES  and  the  cage  position  through  a  set  of  algorithms.  These 
waypoints  were  initially  designed  to  be  fixed  throughout  the  docking  rendezvous 
based  on  fix  cage  heading.  The  design  was  simulated  and  tested  for  all 
conditions  to  determine  the  docking  operation  envelop.  The  next  incremental 
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efforts  in  the  design  were  to  incorporate  the  three  seconds  transmission  delay 
into  the  simulation  followed  by  simulating  a  swinging  cage  dynamics.  The 
simulation  results  were  then  analyzed  to  determine  if  the  proposed  design  is 
satisfactory. 

B.  DOCKING  ACCEPTANCE  CRITERIA 

To  determine  whether  ARIES  has  entered  the  cage  successfully,  first, 
some  details  on  ARIES  and  the  docking  platform  would  need  to  be  known. 
According  to  Johnson  [9],  the  overall  dimensions  of  ARIES  were  measured  as 
follows: 

•  Length  from  nose  to  end  of  tail  section  (not  including  thrusters) 
3.25m 

•  Width  determined  longitudinally  at  the  center  section  0.4m 

•  Height  determined  vertically  at  the  center  section  0.25m 

Based  on  ARIES  dimensions,  the  docking  system  assumed  in  this  study  is 
as  shown  in  Figure  4.  It  has  a  cage  opening  to  receive  the  incoming  AUV  and  a 
docking  tube  to  provide  protective  housing  for  AUV  once  it  has  successfully 
docked.  The  cage  entrance  was  assumed  to  be  conical  in  shape  with  diameter  of 
1  meter  and  apex  angle  of  60°. 

In  most  studies  performed  on  ARIES  previously,  the  authors  treated  the 
position  of  ARIES  as  a  point  located  at  the  vehicle  center  and  were  more 
concerned  on  steering  and  bringing  this  point  to  other  locations.  In  this  study, 
since  docking  into  the  cage  is  the  primary  concern,  the  position  of  the  nose  of 
ARIES  is  also  an  important  parameter  to  monitor.  Therefore,  expressed  in  global 
reference  frame,  the  coordinate  of  vehicle’s  nose  at  any  point  in  time 
is (x (r) +  1.625 cos Y[t)  +  \.625sm.y/{t)) .  The  steering  and  cross  track  error 

control  in  the  simulation,  however,  was  still  about  moving  the  point  at  the  vehicle 
center  to  the  point  at  the  apex  of  the  conical  cage. 

Due  to  the  difficulty  in  simulating  the  constraint  of  motion  cause  by  the 
conical  cage  as  ARIES  made  its  entrance  into  the  cage,  the  physical  constraint 
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caused  by  the  cage  is  not  modeled,  i.e.  in  the  simulation;  ARIES  could  have 
entered  the  cage  and  left  it  from  the  side  of  the  cone  or  it  could  have  entered  the 
cage  from  the  side  of  the  cone.  Instead,  an  acceptance  zone  was  set  such  that 
once  the  nose  of  ARIES  has  entered  this  zone;  it  was  considered  to  have 
entered  the  cage  disregard  of  whether  the  AUV  would  eventually  move  out  of  the 
cage  boundary  in  the  simulation. 

The  acceptance  zone  was  defined  by  two  boundaries: 

1)  ARIES  must  be  within  the  field  of  view  (FOV)  of  the  conical  cage 
and, 

2)  An  imaginary  line  of  0.6  ft  away  from  the  cage  entrance  together 
with  the  cage  entrance  as  shown  in  Figure  4  was  set  as  the 
tolerance  limit.  ARIES  must  fall  within  this  tolerance  limit. 

The  tolerance  limit  was  set  at  0.6ft  to  handle  the  worst-case  situation  due 
to  discretization  in  the  simulation.  The  time  step  for  the  simulation  is  set  at 
0.125sec  which  means  computation  of  the  nose  position  of  ARIES  is  done  at 
every  discrete  time  interval.  The  worst-case  scenario  is  when  =  ^ 

the  nose  position  of  ARIES  is  calculated  just  before  the  cage  entrance.  With  a 
forward  velocity  of  1.4m/s,  the  next  calculated  position  would  be  0.574ft  away 
along  track.  To  give  some  margin,  the  tolerance  limit  was  thus  set  at  0.6ft.  As  the 
general  direction  of  ARIES  is  always  going  towards  the  apex  of  the  cage,  it  would 
also  help  to  eliminate  false  conclusion  of  proper  docking  when  ARIES  closes  into 
the  apex  point  of  the  cage  from  the  side  of  the  conical  cage. 
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FOV  of  cage 


Cage 

ance 


Docking  Tube 

Pictorial  view  of  ARIES  approaching  the  Docking  Platform 


C.  WAYPOINTS  DESIGN 

Waypoints  navigation  is  one  of  the  most  intuitive  autonomous  navigation 
techniques  around  and  has  been  widely  used  in  Aerospace  and  Naval  industries. 
Different  waypoints  can  be  pre-programmed  into  ARIES  to  shape  the  path  that 
ARIES  is  expected  to  follow.  In  practice,  ARIES  could  have  been  programmed 
using  a  list  of  objectives  in  its  mission  file.  These  objectives  are  tasks  that  the 
vehicle  must  achieve  before  moving  on  to  the  next  objective.  An  objective  can 
consist  of  a  set  of  many  waypoints.  Docking  is  therefore,  accomplished  using  the 
“dock”  objective.  Alternatively,  while  ARIES  is  running  a  pre-programmed 
mission,  she  will  be  expected  to  receive  modem  commands  from  other  vehicles 
that  will  trigger  the  “dock”  objective  which  overwrites  the  pre-programmed 
sequence. 

To  design  the  waypoints,  two  important  parameters,  number  of  waypoints 
required  and  position  of  waypoints  would  need  to  be  defined.  Correct  placement 
and  number  of  waypoints  is  important  as  it  will  determine  the  path  towards  the 
cage  entrance  with  sufficient  settling  time  for  the  controller  to  track  the  path.  This 
is  especially  important  when  the  vehicle  is  very  close  to  the  cage  where  path 
tracking  accuracy  is  crucial.  For  a  cage  with  fix  heading,  the  challenge  in  the 
design  of  waypoints  is  the  capability  in  handling  different  vehicle’s  starting 


18 


position  and  heading  in  respect  to  the  docking  cage’s  position  and  heading 
during  the  docking  sequence.  To  achieve  this,  a  parameter  “Diff_angle”  is 
defined  as  the  difference  between  ^//^^^and  the  line  of  sight  angle  between 

ARIES’  starting  point  in  the  docking  sequence  and  apex  point  of  the  conical 
cage,  0  where  Diff_angle  ranges  from  0  to  ±180  and  the  centerline  of  the  cage 
is  used  to  determine  the  cage  heading  angle,  The  range  of  Diff_angle  is 

further  divided  into  12  segments  as  shown  in  Figure  4.  These  segments  are  0°  - 
±30°,  ±30°-  ±60°,  ±60°-  ±90°,  ±90°-  ±120°,  ±120°-  ±150°  and  ±150° 
-  ±  180°.  Depending  on  which  segment  Diff_angle  is  in,  the  number  of  waypoints 
will  vary  as  shown  in  Table  2. 


Diff_angle  Segment 

No.  of  waypoints 

0°  -  ±30° 

1 

±30°  -  ±60° 

2 

±  60°  -  ±  90° 

3 

±90°-  ±120° 

4 

±120°-  ±150° 

5 

±150°-  ±180° 

6 

Table  2.  Allocating  number  of  waypoints 


In  practice,  ARIES  will  automatically  determine  the  number  of  waypoints 
based  on  the  parameter  “diff_angle”.  For  example,  if  -W  <  Diff  _angle<W , 
there  will  only  be  one  waypoint  which  is  located  at  the  apex  of  the  cage.  Since 
ARIES  is  within  the  FOV  of  the  cage,  there  is  no  reason  to  have  more  waypoints 
to  steer  path  of  the  vehicle’s  direction.  However,  for  cases 
^heveDiff  angle  >W  or  Diff  angle  < -30° ,  waypoints  are  needed  to  steer 
ARIES  into  the  right  direction  as  shown  in  Figure  5.  Now,  having  determined  the 
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number  of  waypoints,  the  next  task  is  to  decide  on  the  location  of  these 
waypoints. 


Waypoints  Allocation 


If  ARIES  is  going  to  determine  the  position  of  the  waypoints  automatically, 
certain  relationship  between  the  Diff_angle  segments  and  the  waypoints  will 
need  to  be  established.  One  obvious  choice  is  to  place  the  waypoint  of  each 
segment  along  the  centerline  of  that  segment  with  the  only  two  exceptions  of  the 
last  second  waypoint  along  the  centerline  of  the  cage  and  the  last  waypoint  on 
the  apex  of  the  cage.  With  this  choice,  the  next  decision  is  to  determine  where 
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along  the  centerline  the  waypoint  would  be  set.  Two  possibilities  were  considered 
for  fix  heading  cage: 


•  The  waypoints  were  placed  at  a  length  of  vehicle’s  starting 
distance-to-go  along  the  centerlines  of  each  segments  and  the 
cage.  Thus,  ARIES  would  travel  along  the  track  paths  on  these 
waypoints.  Upon  reaching  the  last  second  waypoint  which  happens 
to  fall  along  the  centerline  of  the  cage,  ARIES  would  steer  itself 
towards  the  last  waypoint  at  the  apex  of  the  cage. 

•  The  second  approach  was  to  set  the  waypoints  spiral  inward 
towards  the  final  waypoint  at  the  apex  of  the  cage.  Laterally,  the 
waypoints  would  still  be  along  the  centerlines  of  each  segments 
and  the  cage  but  length  of  the  waypoint  along  the  centerline 
reduces  to  a  fraction  of  the  previous  waypoint. 


Both  methods  did  have  their  pros  and  cons.  The  latter  method  took  a 
shorter  path  and  time  to  the  cage  but  it  made  sharper  turns  when  ARIES  got 
closer  to  the  cage  and  thus  it  was  not  ideal  for  docking  operation  which  requires 
more  accuracy  and  closer  tracking  of  the  path  when  ARIES  got  closer  to  the 
cage.  This  disadvantage  became  more  evident  for  moving  cage  where  ARIES 
might  end  up  spiraling  to  the  last  waypoint  through  the  side  of  the  conical  cage 
which  in  reality,  meant  a  miss  in  the  docking  process. 

The  former  approach  though  took  a  longer  time  and  traveled  a  long 
distance  before  reaching  the  cage;  it  gave  sufficient  time  for  the  controller’s 
transient  overshoot  to  settle  down.  As  there  is  no  requirement  to  optimize  the 
docking  time,  the  former  approach  was  adopted  for  the  study  and  would  be  used 
in  all  the  subsequent  analysis. 

Watch  radius  is  defined  as  the  tolerance  radius  around  the  waypoint 
whereby  that  waypoint  is  considered  reached  when  ARIES  falls  within  this  radius. 
A  good  design  of  trajectory  path  includes  an  appropriate  choice  of  the  watch 
radius.  For  trajectory  that  has  very  sharp  change  in  direction,  as  can  be  seen 
from  Figure  6,  a  large  watch  radius  is  preferred.  Large  watch  radius  means  that 
vehicle  traveling  along  the  track  to  one  waypoint  will  reach  the  watch  radius  of 
that  waypoint  earlier  and  thus  transit  to  the  next  waypoint  earlier.  This  in  fact, 
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improves  the  tracking  for  sharp  trajectory  path.  For  the  cases  of  a  moving  cage 
or  when  the  vehicle  is  very  close  to  the  cage,  a  large  watch  radius  can  mean  a 
loss  in  tracking  accuracy  and  thus  missing  the  cage’s  entrance.  Therefore, 
smaller  watch  radii  are  preferred  in  these  cases.  In  the  simulation  the  watch 
radius  started  from  10m  and  reduces  as  the  waypoints  get  closer  to  the  cage 
since  terminal  accuracy  is  of  outmost  important  in  docking  application.  It  was 
also  reckoned  that  for  fixed  cage  heading,  the  sharp  trajectory  usually  happens 
at  the  last  second  waypoint  where  the  vehicle  is  trying  to  steer  a  sharp  turn 
inward  towards  the  cage.  Depending  on  where  with  respect  to  the  position  of  the 
cage  ARIES  is  when  the  docking  sequence  is  activated,  the  last  second  waypoint 
is  always  of  the  same  distance  away  from  the  cage  as  the  starting  point.  As  long 
as  certain  minimum  distance  constraint  is  placed  on  the  distance  of  starting  point 
away  from  the  cage  such  that  it  is  far  enough  to  give  sufficient  time  for  the 
tracking  transient  overshoot  to  settle  down,  the  waypoints  design  for  the  docking 
process  is  completed. 
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D.  SIMULATING  DELAYED  DATA  TRANSFER 

Thus  far,  it  was  assumed  that  the  data  transfer  between  the  acoustic 
modem  of  docking  platform  and  ARIES  was  instantaneous.  Unfortunately,  Marr’s 
experimental  study  [6]  showed  that  the  minimum  acoustic  data  transfer  rate  for 
ARIES  is  about  0.3  Hz.  That  was  equivalent  to  one  update  per  3  second.  The 
update  rate  of  0.3  Hz  can  be  easily  simulated  using  sample  and  hold  technique 
where  cage  heading  data  is  held  constant  for  3  seconds  between  updates.  This 
update  which  contains  the  Cage  heading  serves  as  the  commanded 

reference  input  for  the  horizontal  steering  control.  For  fixed  cage  heading,  a 
delayed  data  transfer  does  not  affect  the  result.  However,  for  moving  cage,  with 
erroneous  commanded  reference  input,  ARIES  will  steer  towards  the  wrong  point 
within  the  updates.  This  will  persist  until  a  new  measurement  update  is  obtained 
for  the  cage  heading  which  can  be  used  to  correct  ARIES’  path.  Two  obvious 
implications  of  such  implementation  are 

•  As  the  ARIES  approaches  the  last  three  second  of  the  docking 
operation,  it  will  not  have  anymore  cage  heading  updates.  This  can 
cause  ARIES  to  miss  the  cage  entrance  if  the  cage  is  swinging  at 
high  amplitude  and  frequency. 

•  If  the  cage  is  swinging  at  high  rate,  with  limited  forward  speed  of 
ARIES  coupled  with  erroneous  commanded  reference  input,  ARIES 
can  be  easily  out-maneuvered. 

E.  DYNAMIC  WAYPOINTS  ALLOCATION  (DWA) 

Dynamic  waypoints  allocation  is  a  term  coined  to  describe  continuous 
allocation  of  waypoints  based  on  ARIES  and  cage’s  position  and  heading.  This 
implementation  is  especially  useful  for  cage  in  motion  as  ARIES  can  adjust  its 
waypoints  according  to  the  cage  heading  when  it  approaches  the  cage.  The 
methodology  of  where  to  allocate  the  waypoints  and  number  of  waypoints  to  be 
allocated  is  as  per  stated  in  section  C.  The  only  different  is  these  waypoints 
allocation  changes  based  on  current  ARIES  and  cage’s  position  and  heading.  In 
the  case  where  update  rate  of  0.3  Hz  is  considered,  the  allocation  of  waypoints 
will  be  done  every  3  seconds  when  new  cage’s  data  arrive. 
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F.  MOVING  CAGE  DYNAMICS 

The  problem  of  homing  ARIES  into  a  cage  that  has  fixed  heading  but 
oscillating  in  horizontal  position  is  very  different  from  one  where  the  cage  is  in 
fixed  position  but  swinging  about  a  pivot  point.  The  former  is  a  tracking  problem 
in  the  horizontal  plane  while  the  latter  is  a  steering  problem  to  minimum  the  cross 
track  error  and  LOS  error.  With  varying  cage’s  heading,  the  steering  problem  is 
made  more  complicate  as  shown  in  Figure  7.  For  a  27s  turn  rate  of  the  cage,  at 
25m  away  in  order  for  ARIES  to  keep  track  with  it,  ARIES  need  to  travel  at  about 
0.87  m/s.  At  100m  away,  the  forward  velocity  requirement  increases  to  3.49  m/s. 
For  a  turn  rate  of  37s,  at  100m  away,  the  forward  velocity  requirement  of  5.24 
m/s  has  exceeded  ARIES  maximum  velocity  of  3.5  m/s.  Based  on  the  worst  case 
scenario  illustrated  in  Figure  7  and  assuming  that  the  docking  sequence  is 
typically  within  100m,  the  turn  rate  of  the  cage  should  not  be  more  than  2-37s.  In 
addition,  a  cage  that  tends  to  change  its  direction  very  often  would  be  harder  for 
ARIES  to  dock.  This  is  because  based  on  DWA  method;  ARIES  has  quite  a  large 
turn  radius  and  that  sluggishness  limits  it  to  have  a  fast  change  in  its  direction. 
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Impact  of  Turning  Rate  of  Cage  on  the  Requirement  of  Vehicle’s 

Maneuverability 


In  this  study,  the  cage  motion  in  the  horizontal  plane  is  first  assumed  to  go 
in  one  direction  at  a  rate  of  27s.  Subsequently,  a  sinusoidal  swinging  motion 
about  a  pivot  located  at  the  apex  of  the  cage  is  then  considered.  The  cage 
motion  thus  can  be  modeled  with  the  resulting  dynamics  matrix: 


"  1  0^ 


(47) 


To  test  the  steering  controller,  a  model  with  a  period  of  180  seconds  and 
amplitude  of  60°  was  chosen.  The  resulting  frequency  is  0.035  rad/sec. 
Implementing  in  Matlab,  the  cage  dynamics  is 

initial_cage_psi  =  60*sin(0.035*time). 
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IV.  SIMULATION  RESULTS 


A.  FIXED  CAGE  HEADING 

The  simulation  was  performed  incrementally  starting  from  a  fixed  cage 
heading  scenario.  Different  simulation  runs  were  designed  to  test  the  limitation 
and  performance  of  the  ARIES  under  different  initial  conditions.  The  different 
simulations  conducted  and  their  test  objectives  were  summarized  in  table  3. 


Sim 

No. 

ARIES 

Forward 

Velocity 

ARIES  Initial 

Position  (m) 

ARIES  Initial 

Heading 

Cage 

Position 

(m) 

Cage 

Heading 

Test 

Objective 

1 

1 .4  m/s 

(110,80) 

180° 

(10,100) 

0° 

Effectiveness  of 

waypoints 

2 

1 .4  m/s 

(110,80) 

180° 

(10,100) 

145° 

Effectiveness  of 

waypoints 

3 

1 .4  m/s 

(65,100) 

0° 

(10,100) 

180° 

Minimum  Distance 

requirement 

4 

1 .4  m/s 

(50,100) 

0° 

(10,100) 

180° 

Minimum  Distance 

requirement 

5 

1 .4  m/s 

(50,100) 

0°,  50°, 

180°,  270° 

(10,100) 

180° 

Impact  of  Initial 

heading 

6 

0.5  m/s 

(19,100) 

50° 

(10,100) 

180° 

Impact  of  different 

initial  forward  velocity 

(watch  radius  set  to 

2m) 

7 

2.5  m/s 

(145,100) 

50° 

(10,100) 

180° 

Impact  of  different 

initial  forward  velocity 

Table  3.  List  of  Simulation  Runs  for  Fixed  Cage  Heading 
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Simulation  #  1  and  2  were  designed  to  verify  the  trajectory  design  and 
docking  testing  conditions  stipulated  in  chapter  3.  Distance  between  the  initial 
ARIES  position  and  cage’s  position  was  set  such  that  there  were  sufficient  time 
for  the  controller  to  settle  down  from  its  transient  overshoot  after  switching  from 
one  waypoint  to  the  other.  Simulation  #1  has  ARIES  initial  docking  position 
placed  within  the  FOV  of  the  cage.  Under  such  case,  ARIES  should  follow  the 
line-of-sight  track  towards  the  cage  entrance.  In  Simulation  #2,  since  the 
Diff_angle  is  156.3°,  there  would  be  six  waypoints.  Though  the  initial  heading  of 
ARIES  was  not  aligned  with  the  pre-planned  path,  the  LOS  error  and  cross  track 
error  controller  steered  ARIES  towards  the  pre-planned  path  subsequently. 
Another  observation  made  was  that  the  last  second  waypoint  always  made  a 
very  sharp  turn  inward.  That  created  a  large  transient  overshoot  for  the  controller 
and  it  took  a  substantial  amount  of  time  to  settle  back  to  its  intended  track.  This 
characteristic  was  thus  the  key  determinant  on  the  minimum  distance  from  the 
cage  that  the  docking  sequence  could  be  activated.  The  results  of  simulation  #1 
and  2  were  plotted  in  Figure  7  and  8.  In  both  cases,  ARIES  managed  to  dock  into 
the  cage  successfully. 
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Simulation  #3  and  #4  attempted  to  determine  the  minimum  required 
distance  between  the  ARIES  starting  position  to  the  cage  position.  Starting  from 
a  distance  of  100m  away,  the  distance  was  reduced  with  the  rest  of  the 
parameters  kept  constant.  At  a  distance  55m  away,  as  shown  in  Figure  9,  ARIES 
barely  made  it  to  the  cage  entrance.  With  the  rest  of  the  parameters  remained 
the  same,  at  40m  away,  ARIES  could  not  make  it  to  the  cage  as  shown  in  Figure 
10.  The  minimum  required  distance  was  found  to  be  around  55m  away  from  the 
cage. 


ARIES:  (65,100)  at  0°  @  1.4  m/s.  Cage:  (10,100)  at  180°  (Fixed  cage 

Scenario) 
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ARIES:  (50,100)  at  0°  @1.4  m/s,  Cage:  (10,100)  at  180°  (Fixed  cage 

Scenario) 


Next,  the  initial  vehicle  heading  was  varied  in  simulation  #5  with  the  rest  of 
the  parameters  kept  at: 

•  AREIS  Forward  Velocity  =  1.4  m/s 

•  ARIES  Starting  Position  =  (50,100) 

•  Cage  Position  =  (10,100) 

•  Cage  Fleading  =  180° 

This  was  done  to  determine  the  impact  of  initial  vehicle  heading  on  the  terminal 
steering  accuracy.  It  was  found  that  when  the  initial  vehicle  heading  was  180°  off 
the  intended  path,  the  controller  created  the  highest  error  as  shown  in  Figure  11 
and  thus  presented  the  worst-case  scenario.  Therefore,  in  subsequent 
simulations,  the  initial  vehicle  heading  was  set  at  this  condition  so  that  the  most 
critical  condition  was  tested. 
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ARIES:  (50,100)  at  0°,  50°,  180°  and  270°  @1.4  m/s.  Cage:  (10,100)  at  0° 

(Fixed  cage  Scenario) 


One  of  the  observations  made  from  the  simulations  conducted  so  far  was 
that  the  turn  radius  made  by  the  steering  controller  was  large.  Referring  back  to 
the  equations  of  motion,  the  forward  velocity  of  the  vehicle  did  play  a  part  in 
influencing  the  turn  radius.  The  higher  the  forward  velocity,  the  larger  would  be 
the  turn  radius.  Therefore,  in  simulation  #  6  and  7,  the  impact  of  varying  the 
vehicle’s  forward  velocity  was  investigated.  It  was  found  that  the  minimum 
distance  required  reduced  to  9m  for  forward  velocity  of  0.5  m/s  versus  a 
minimum  distance  of  135m  for  forward  velocity  of  2.5  m/s.  The  conclusion  was 
therefore,  for  fixed  cage  heading  where  timing  for  docking  was  not  critical;  a 
slower  vehicle’s  forward  velocity  was  desired  in  order  to  overcome  a  sharper  turn 
radius.  To  put  this  finding  into  implementation,  a  lookup  table  of  vehicle’s  forward 
velocity  could  be  created  for  different  vehicle  to  cage  distance. 
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ARIES:  (19,100)  at  50°  @  0.5  m/s,  Cage;  (10,100)  at  180°  (Fixed  cage 

Scenario) 


ARIES:  (145,100)  at  50°  @  2.5  m/s.  Cage:  (10,100)  at  180°  (Fixed  cage 

Scenario) 
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B.  DYNAMIC  WAYPOINT  ALLOCATION 

In  chapter  3,  the  concept  of  dynamic  waypoint  allocation  (DWA)  was 
introduced.  The  results  and  performance  of  such  implementation  are  discussed 
in  this  section. 

To  study  the  performance  of  allocation  of  waypoint  based  on  relative 
heading  and  position  of  vehicle  and  cage  dynamically,  the  cage  heading  was  first 
kept  constant  throughout  the  simulation.  For  a  start,  more  benign  initial 
conditions  were  set  with  ARIES’  starting  position  set  at  100m  away,  forward 
velocity  1.4  m/s  and,  both  cage  and  vehicle  headings  set  at  180°.  The  waypoints 
were  updated  every  3  sec  based  on  the  relative  heading  and  position  of  the 
vehicle  and  cage  at  that  instant.  The  result  of  the  successful  docking  was 
showed  in  Figure  14.  It  was  also  observed  in  Figure  15  that  when  comparing  the 
trajectory  path  between  fixed  and  dynamic  waypoints  allocation,  the  latter  took  a 
shorter  and  smaller  turn  radius  path  since  it  incrementally  corrected  its  heading 
every  3  sec  as  ARIES  approached  the  cage  entrance.  The  result  also  showed  a 
smoother  path  and  smaller  transient  overshoot  even  ARIES  made  a  sharp 
maneuver.  Being  able  to  reduce  the  transient  overshoot  was  keyed  in  the 
reduction  of  minimum  vehicle-cage  starting  distance  apart. 

The  next  simulation  conducted  was  to  identify  the  impact  of  initial  vehicle 
heading  on  the  terminal  steering  accuracy.  Figure  16  showed  the  result  for  initial 
vehicle  headings  of  0°,  90°,  180°  and  270°  where  vehicle-cage  distance  is  40m 
apart  and  initial  cage  heading  at  180°.  Though  all  the  four  initial  vehicle  headings 
managed  to  dock  into  the  cage  successfully,  the  run  with  initial  heading  of  180° 
just  marginally  met  the  docking  requirement.  This  presented  the  most  critical 
initial  heading  since  ARIES  needed  to  make  the  tightest  turn  and  it  nearly  missed 
the  cage.  The  most  critical  initial  vehicle  heading  thus  is  when  ARIES’  initial 
heading  has  the  same  angle  as  the  initial  cage  heading. 
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ARIES:  (110,100)  at  180°  @  2.5  m/s,  Cage:  (10,100)  at  180°  (Fixed  cage  with 
Dynamic  waypoint  allocation  Scenario) 


Comparison  between  Fixed  Waypoint  allocation  and  Dynamic  Waypoint 

Allocation  Technique 
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Impact  of  Initial  Vehicle  Heading  on  Terminal  Accuracy  of  Dynamic  Waypoints 

Allocation  Method 

To  further  prove  that  DWA  method  was  more  superior  in  performance  as 
compared  to  fixed  waypoint  allocation  method,  simulations  using  both  methods 
were  ran  at  conditions  where  ARIES  failed  to  dock  under  the  fixed  waypoint 
allocation  method.  At  vehicle’s  forward  velocity  of  1.4  m/s,  vehicle-cage  distance 
of  40  m/s,  and  initial  vehicle  and  cage  heading  of  180°,  the  DWA  method  proved 
to  be  more  superior  as  it  managed  to  dock  successfully.  This  has  a  lot  to  do  with 
the  reduction  of  transient  overshoot  when  using  the  DWA  method.  Figure  17 
showed  the  performance  comparison  of  both  methods. 
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Performance  Comparison  between  Dynamic  and  Fixed  Waypoints  Allocation 

Methods 
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However,  at  forward  velocity  of  0.5  m/s  as  seen  in  Figure  18,  the  minimum 
vehicle-cage  distance  achieved  for  the  DWA  case  was  only  20m  as  compared  to 
fixed  waypoint  allocation  of  9m.  That  was  again  caused  by  the  tight  turn  radius 
which  is  the  characteristics  of  DWA  method.  The  DWA  method  though  has  the 
turn  radius  constraint  at  low  vehicle  forward  velocity  which  capped  the  minimum 
vehicle-cage  distance  at  higher  value,  its  ability  of  always  adjusting  itself  based 
on  dynamic  conditions  of  both  the  cage  and  vehicle  far  outweighed  its  shortfall. 
DWA  method  is  the  obvious  choice  for  cage  with  swinging  motion  scenario. 
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ARIES:  (30,100)  at  180°  @  0.5  m/s,  Cage:  (10,100)  at  180°  (Fixed  cage. 

Dynamic  Waypoints  Scenario) 
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C.  SWINGING  CAGE  SCENARIO 

1.  Swinging  Cage  at  a  rate  of  2°ls  in  Singie  Direction  (Anti- 
Ciockwise) 

The  seemingly  straightforward  swinging  cage  problem  was  in  fact  made 
challenging  with  the  constraint  imposed  in  ARIES’  forward  velocity  coupled  with 
the  tighter  requirement  on  turn  radius  as  ARIES  approached  the  cage.  The 
problem  was  not  only  on  tracking  the  turn  angle  of  the  cage  but  also  when  and 
how  much  to  close  in  to  the  cage  taking  into  consideration  the  turn  radius  of 
ARIES.  The  primary  parameters  that  affected  the  docking  accuracy  include 
vehicle  forward  velocity,  waypoint  watch  radius  and  location,  and  vehicle  turn 
radius.  Different  vehicle-cage  distance,  initial  vehicle  and  cage  heading  also 
affected  the  trajectory  path  of  ARIES  towards  the  cage  and  thus  these 
parameters  were  closely  interrelated  with  some  of  the  primary  influencing 
parameters.  It  was  found  through  iterative  method  that  at  forward  velocity  of 
about  1 .9  m/s,  a  good  trade-off  between  forward  velocity  and  required  turn  radius 
was  achieved.  Also,  in  cases  where  diff  angle  >60°  or diff  _angle  < -60° ,  instead 
of  traveling  along  the  track  path  of  waypoints  which  have  same  distance  to  the 
cage  as  the  vehicle,  the  last  two  waypoints  were  set  at  80%  of  the  vehicle-cage 
distance.  That  implementation  was  tailored  to  allow  a  faster  convergence  into  the 
cage. 

The  simulation  was  running  at  a  time  step  of  0.05  sec  and  for  a  start, 
perfect  sensor  updates  of  the  cage’s  data  were  assumed.  Also,  in  the  simulation, 
the  cage  has  a  turn  rate  of  27s  rotating  in  anti-clockwise  direction.  Different 
simulation  runs  were  performed  to  verify  the  ability  of  the  design  in  steering  into  a 
rotating  cage. 
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Sim 

No. 

ARIES 

Forward 

Velocity 

ARIES  Initial 

Position  (m) 

ARIES  Initial 

Heading 

Cage 

Position 

(m) 

Cage 

Starting 

Heading 

Test 

Objective 

8 

1.9  m/s 

(100,0) 

0° 

(200,0) 

180° 

Impact  of  initial 

heading  on  the 

accuracy  of  docking 

9 

1.9  m/s 

(100,0) 

180° 

(200,0) 

0° 

Impact  of  initial 

heading  on  the 

accuracy  of  docking 

10 

1.9  m/s 

(100,0) 

90° 

(200,0) 

270° 

Impact  of  initial 

heading  on  the 

accuracy  of  docking 

Table  4.  List  of  Simulation  Runs  for  Rotating  Cage 


All  the  three  simulations  in  Table  4  were  performed  with  ARIES  having  a 
docking  distance  of  100m.  The  only  differences  were  the  initial  heading  of  ARIES 
and  the  docking  cage.  From  the  simulations,  it  was  found  that  in  all  three  cases 
as  shown  in  Figure  20-22,  ARIES  managed  to  dock  successfully.  Flowever,  it 
was  observed  that  for  simulation  #10  as  shown  in  Figure  22,  ARIES  made 
multiple  attempts  before  it  could  finally  dock  into  it.  Further  investigation  on  this 
set  of  initial  condition  showed  that  it  happened  to  be  the  worst-case  scenario. 
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ARIES:  (100,0)  at  0°  @  1 .9  m/s,  Cage;  (200,0)  at  initial  heading  of  180°  at 
2°/s  Anti-clockwise  (Swinging  cage.  Dynamic  Waypoints  Scenario) 


ARIES:  (100,0)  at  180°  @  1.9  m/s.  Cage:  (200,0)  at  initial  heading  of  0°  at 
2°/s  Anti-clockwise  (Swinging  cage.  Dynamic  Waypoints  Scenario) 
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ARIES:  (100,0)  at  90°  @  1 .9  m/s,  Cage:  (200,0)  at  initial  heading  of  270°  at 
2°/s  Anti-clockwise  (Swinging  cage.  Dynamic  Waypoints  Scenario) 

2.  Swinging  Cage  at  a  rate  of  2°ls  and  Oscillating  with  Amplitude 
of  60° 

The  next  simulation  (Simulation  #11)  targeted  on  the  ability  of  the  design 
in  docking  into  swinging  cage  with  oscillating  motion.  This  is  one  of  the  more 
likely  undersea  cage  motions.  In  the  simulation,  the  cage  was  set  to  rotate  in 
oscillatory  motion  of  amplitude  60°  and  a  turn  rate  of  2°/s.  Figure  23  showed  a 
successful  docking  result  for  starting  docking  distance  of  150m. 
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ARIES:  (50,0)  at  0°  @  1 .9  m/s,  Cage:  (200,0)  at  initial  heading  of  180°  at  2°/s 
oscillating  with  60°  (Swinging  cage.  Dynamic  Waypoints  Scenario) 

3.  Swinging  Cage  at  a  rate  of  2°ls  (With  Data  Transfer  Delay  at 
3sec) 

The  final  set  of  simulations  were  to  re-run  simulation  #8-11  but  this  time 
with  data  from  the  cage  updated  only  once  every  3  sec.  The  results  in  figure  24- 
27  showed  that  out  of  the  4  simulations,  only  simulation  #9  managed  to  dock 
successfully.  This  set  of  simulations  gave  a  good  indication  on  the  impact  of  a 
3sec  delay  data  on  the  accuracy  of  the  final  docking  which  led  us  to  the  solution 
elaborated  in  the  next  chapter. 
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ARIES:  (100,0)  at  0°  @  1 .9  m/s,  Cage:  (200,0)  at  initial  heading  of  180°  at 
2°/s  anti-clockwise  (Swinging  cage,  3  sec  Delayed  Scenario) 


ARIES:  (100,0)  at  180°  @  1.9  m/s.  Cage:  (200,0)  at  initial  heading  of  0°  at 
2°/s  anti-clockwise  (Swinging  cage,  3  sec  Delayed  Scenario) 
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Y  Coordinate  (m)  (/>  Y  Coordinate  (m) 


:  (1 00,0)  at  90°  @  1 .9  m/s,  Cage;  (200,0)  at  initial  heading  of  270°  at 
2°/s  anti-clockwise  (Swinging  cage,  3  sec  Delayed  Scenario) 


ARIES:  (50,0)  at  0°  @  1 .9  m/s.  Cage;  (200,0)  at  initial  heading  of  180°  at  2°/s 
oscillating  with  60°  (Swinging  cage,  3  sec  Delayed  Scenario) 
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V.  SOLUTIONS  TO  DELAYED  DATA  TRANSFER 


In  the  absence  of  any  data  from  the  cage  with  the  3  sec  updates,  most  of 
the  estimation  techniques  which  use  a  higher  update  rate  sensor  to  predict  the 
lower  update  rate  sensor’s  data  like  complimentary  filtering  or  Kalman  filtering 
would  not  work.  In  the  previous  implementation,  the  3  sec  transmission  delay 
was  simulated  using  sample  and  hold  technique  which  meant  that  within  the  3 
seconds  update  ARIES  would  be  steering  to  the  last  update  cage  heading  until  a 
new  data  updates  appeared.  That  implementation  could  be  jerky  since  every 
update  would  create  a  sudden  jump  in  steering  command  and  that  was 
especially  true  if  the  cage  was  turning  at  higher  rate.  Also  as  mentioned  before, 
at  the  terminal  approach,  ARIES  could  be  not  receiving  any  update  for  up  to  3 
seconds  which  was  yet  another  source  of  error.  In  addition  the  sample  and  hold 
technique  always  left  the  controller  chasing  after  the  new  cage  heading  on  every 
new  updates  which  could  be  too  late  for  ARIES  to  react. 

The  proposed  solution  is  to  patch  and  smooth  out  the  emptiness  in 
between  the  3  second  updates  with  predicted  cage  ip  angle  value.  One  of  the 
techniques  that  can  be  adopted  is  first  order  Euler  integration.  This  method  is 
greatly  used  in  simulation  software  like  in  Simulink  solver.  Basically,  the  new  ip 
estimated  in  every  time  step  in  the  simulation  can  be  computed  using 

¥ne^=¥oM+¥oMdt  (48) 

Where: 

dt  =  time  step  in  the  simulation 

=  Cage  rotation  rate  (Positive  -  Anti-clockwise  direction) 

In  the  above  implementation,  the  assumption  is  that  the  cage  is  rotating  at 

either  a  fix  rate  or  slow  rate  and  thus  within  the  3  seconds  of  data  updates,  a 

constant  cage  rotation  rate  base  on  last  update  is  used  to  predict  y/^^\r\  the 

next  three  seconds.  To  implement  this,  data  transferred  from  the  cage  must 

include  both  the  cage  heading  and  its  rotation  rate.  For  a  cage  rotation  rate  of  2- 
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37s,  this  technique  should  be  sufficient  and  the  implementation  is  as  shown  in 
Appendix  C. 

In  fact  as  expected,  if  the  model  of  the  dynamics  of  the  cage  is  known,  the 
Euler  integration  method  is  essentially  performing  equally  well  as  if  there  is  no 
data  transmission  delay.  The  new  cage  y/  calculated  at  each  simulation  time 
step  just  need  to  follow  certain  reference  cage  model  and  ARIES  will  have  exact 
cage’s  data  at  every  simulation  time  step.  However,  perfect  knowledge  of  the 
cage’s  dynamics  and  model  at  all  time  is  not  possible  in  most  cases.  Therefore, 
more  simulations  were  done  to  validate  the  effectiveness  of  this  method. 

For  a  single  directional  rotating  cage,  in  order  to  add  in  more  reality  in  the 
simulation,  a  randomizer  varying  from  0  to  2  was  added  to  the  rotation  rate  of  the 
cage.  That  was  included  to  further  verify  the  robustness  of  the  design  under 
uncertain  rotation  rate.  Figure  28  showed  that  even  under  slight  randomness  in 
the  rotation  rate,  the  design  is  robust  enough  for  ARIES  to  dock  successfully. 


X  Coordinate  (m) 


ARIES:  (100,0)  at  0°  @  1 .9  m/s.  Cage:  (200,0)  at  initial  heading  of  180°  at 
2°/s  anti-clockwise  (3  sec  Delayed,  with  randomizer,  Euler  Integration 

implemented  Scenario) 
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Pushing  the  rotation  rate  boundary,  in  the  next  simulation,  the  rotation  rate  was 
set  to  57s  with  randomizer  as  well  to  create  the  uncertain.  The  result  in  Figure  29 
showed  that  with  sufficient  time  given,  the  design  is  robust  enough  to  dock  into 
the  cage  after  multiple  attempts. 


ARIES:  (100,0)  at  0°  @  1 .9  m/s.  Cage:  (200,0)  at  initial  heading  of  180°  at 
5°/s  anti-clockwise  (3  sec  Delayed,  with  randomizer,  Euler  Integration 

implemented  Scenario) 

So  far  in  the  simulation,  the  motion  considered  was  in  single  direction.  Finally, 
the  next  simulation  was  conducted  to  examine  the  robustness  of  the  design  in 
handling  oscillatory  swinging  cage.  Since  a  sine  term  that  is  non-linear  was 
included  to  simulate  the  oscillatory  motion  and  Euler  integration  is  linear  in 
nature,  more  error  between  the  actual  and  predicted  cage  heading  was 
expected.  In  addition,  when  the  actual  cage  heading  changed  direction  in 
between  updates,  the  discrepancy  between  actual  and  predicted  cage  heading 
widened  until  the  next  update  where  the  predicted  heading,  rotation  rate  and 
direction  of  turn  were  corrected.  For  the  case  of  2°/s  rotation  rate,  under  the 
worst-case  situation,  the  maximum  error  possible  was  12°  and  depending  on 
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when  this  error  occur  during  the  simulation,  it  could  cause  ARIES  to  miss  the 
cage  entrance.  However,  in  most  cases,  the  error  caused  by  the  change  in 
direction  would  be  much  less  that  12°  and  also,  for  this  error  to  do  any  impact  on 
the  accuracy,  this  error  would  need  to  happen  when  the  vehicle  was  very  close  to 
the  cage.  The  result  for  swinging  cage  at  2°/s  oscillating  with  60°  amplitude  and 
initial  condition  of  ARIES:  (100,0)  at  0°  @  1.9  m/s.  Cage:  (200,0)  at  initial 
heading  of  180°  was  shown  in  Figure  30  which  demonstrated  that  the  design  is 
still  robust  enough  to  handle  a  slow  oscillatory  motion  in  this  case. 


ARIES:  (100,0)  at  0°  @  1 .9  m/s.  Cage:  (200,0)  at  initial  heading  of  180°  at 
2°/s  oscillating  with  60°  (Swinging  cage,  Euler  Integration  implemented 

Scenario) 
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VI.  CONCLUSION  AND  RECOMMENDATIONS 


A.  CONCLUSION 

The  problem  of  horizontal  steering  control  of  ARIES  into  a  swinging  cage 
is  very  complex.  Many  factors  affect  the  final  outcome  of  the  docking  results. 
These  factors  include  the  choice  of  controller,  position  and  number  of  waypoints, 
how  often  the  waypoints  are  updated,  the  choice  of  watch  radius,  vehicle  forward 
velocity,  vehicle  turn  radius,  initial  conditions  that  affect  the  trajectory  and  finally 
the  data  transmission  rate  of  the  acoustic  modem. 

This  thesis  has  shown  that  cross  track  error  and  LOS  error  controllers 
together  with  a  dynamic  waypoints  allocation  technique  if  properly  tuned,  can  be 
effective  in  steering  ARIES  into  a  cage  with  slow  swinging  motion.  The  design 
has  also  proven  to  be  fairly  robust  in  different  initial  conditions.  However,  this 
design  starts  to  be  less  reliable  and  predictable  when  the  rotation  rate  of  the 
cage  increases.  One  of  the  reasons  is  that  as  the  rotation  rate  of  the  cage 
increases,  the  AUV  cannot  keep  up  with  it  and  the  vehicle  can  end  up  closer  to 
the  cage  but  in  the  wrong  direction.  This  will  require  the  vehicle  to  make  a  sharp 
turn  around  the  cage,  which  in  most  cases  exceeds  the  vehicle  turning  radius 
capability,  and  thus  constitute  to  a  miss  attempt. 

In  addition,  this  thesis  shows  that  the  3  seconds  transmission  delay  does 
have  a  negative  impact  on  the  final  outcome  of  the  docking.  Thus,  in  the  absence 
of  cage  data  updates,  this  thesis  attempts  to  use  an  Euler  integration  technique 
to  predict  the  cage  heading.  The  result  shows  that  for  scenarios  where  the 
docking  cage  has  a  low  rotation  rate,  in  most  cases  the  Euler  integration 
technique  can  predict  the  cage  heading  with  reasonable  accuracy  and  contribute 
positively  to  the  success  of  docking. 
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B.  RECOMMENDATIONS 

One  of  the  problems  encountered  in  the  horizontal  steering  is  that  when 
the  cage  turns,  the  current  design  requires  ARIES  to  make  a  large  steer  when  it 
was  still  a  long  distance  away  from  the  cage  in  order  to  keep  track  with  the  cage 
heading.  This  is  demanding  for  ARIES,  which  has  a  limited  forward  velocity  and 
thus  limits  the  feasibility  of  the  design  for  a  cage  with  a  fast  rotation  rate.  Further 
research  can  be  done  to  investigate  algorithms  that  will  approach  directly  the 
cage  and  only  steer  towards  the  cage  entrance  when  the  vehicle  is  closer  to  the 
cage.  This  optimal  distance  where  such  steering  begins  can  depend  on  the 
rotation  rate  of  the  cage  and  the  cage  heading  when  ARIES  reaches  it.  In 
addition,  more  research  can  be  conducted  to  couple  the  current  design  described 
in  this  thesis  with  a  speed  controller  which  can  regulate  the  forward  velocity  of 
ARIES  as  it  approaches  the  cage.  Finally,  besides  using  just  the  acoustic  modem 
for  data  transmission,  more  researches  in  other  forms  of  data  transmission  with 
higher  update  rate  or  guidance  method  such  as  electromagnetic  guidance  [13] 
can  be  conducted  to  resolve  the  problem  of  the  limitation  in  the  data  transfer  rate 
of  the  acoustic  modem. 
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APPENDIX  A:  MATLAB  FILE  M0TI0N3D.M 


clear 

cic 

TRUE  =  1; 

FALSE  =  0; 
check_condition  =0; 

DegRad  =  pi/180; 

xcg  =  5.33;  %  Length  from  tip  of  the  nose  to  CG  position  in  X-co-ordinate 

(assuming  xcg=0.5xLength  of  Aries 
%  Length  of  Aries  =  128  inches  =  10.67ft.  xcg=10.67/2  =  5.33ft 

%  Jay  Johnson  Model 
m  =  222.26; 

U  =  1 .4*3.28;  %  Forward  Speed 

Yv  =-68.16; 

Yr  =  406.3; 

Ydr  =  70.0; 

Nv  =-10.89; 

Nr  =  -88.34; 

Ndr  =  -35.47; 

MY  =  456.76; 

IN  =  215; 

M  =  diag([MY,IN,1]); 

AA  =  [Yv,Yr-(m*U),0;Nv,Nr,0;0,1 ,0]; 

BB  =  [Ydr;Ndr;0]; 

A  =  inv(M)*AA; 

B  =  inv(M)*BB; 

%  Below  are  in  British  Units  for  CTE  Sliding  Mode 

Lami  =2.0; 

Lam2=  1.0; 

cagejength  =3/3.28;  %  length  cone  of  the  cage  in  meter 

Eta_FlightHeading  =  1.0; 

Phi_FlightHeading  =  0.5; 

%  Below  for  tanh 

Eta_CTE  =  0.1; 

Eta_CTE_Min  =  1 .0; 

Phi_CTE  =  0.5; 

Uc  =  []; 

Vc  =  []; 

Sigma  =  []; 

Depth_com  =  []; 
dr=[]; 
a  =  -.3; 
b  =  (9/24)*a; 

dt  =  0.125;  %  delta  T  per  iteration 
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t  =  [0:dt:1000]'; 
time  =  0; 
room  =  0.0; 

SurfaceTime  =  25; 

%  Set  initial  conditions 

v(1)  =0.0;  %  Initial  Side  Slip  Velocity 

r(1)  =0.0;  %  Initial  Yaw 

rRM(1)  =  r(1); 

Xpos  =  input('lnitial  X  position  of  vehicle  in  meters  (Default:100.0) :');  %  Initial  Position  in  meters 
if  isempty(Xpos), 

X(1)=100.0; 

else 

X(1)=Xpos; 

end 

Ypos  =  input('lnitial  Y  position  of  vehicle  in  meters  (Default:0.0)  :');  %  Initial  Position  in  meters 
if  isempty(Ypos), 

Y(1)=0.0; 

else 

Y(1)=Ypos; 

end 

psideg  =input('lnitial  vehicle  orientation  in  degree  (Default:0.0) :');  %  Initial  vehicle  orientation 
if  isempty(psideg), 
psi(1)=0.0; 
else 

psi(1)=psideg; 

end 

psi(1)  =  psi(1)*DegRad;  %  Initial  Heading  of  ARIES 

W_R(1)  =  1 .0;  %  Sets  initial  Watch  Radius 

x(:,1)  =  [v(1);r(1);psi(1)]; 

SurfPhase(l)  =  0; 

Notracks  =1 ; 

Depth_com(1)  =  5.0; 

WayPointVertDistcom  =  5.0; 

SURFACE_TIMER_ACTIVE  =  FALSE; 

Xcagejnitial  =  input('lnitial  X  position  of  cage  in  meters  (Default:200.0) :');  %  Initial  Position  of 
cage  in  meters 
if  isempty(Xcage_initial), 

Xcage_initial=200.0; 

end 

Ycagejnitial  =  input('lnitial  Y  position  of  cage  in  meters  (Default:0.0)  :');  %  Initial  Position  of  cage 
in  meters 

if  isempty(Ycage_initial), 

Ycage_initial=0.0; 

end 

initial_cage_psi  =  input('lnitial  cage  orientation  in  degree  from  0-360  (Default:! 80.0) :');  %  Initial 
cage  orientation 
if  isempty(initial_cage_psi), 
initial_cage_psi=180.0; 
initial_cage_psi=mod(initial_cage_psi,360); 
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end 


%  Set  start  position 

X_Way_c(1)  =  Xcagejnitial;  %  current  waypoint  is  the  cage  position 
Y_Way_c(1)  =  Ycagejnitial; 

PrevX_Way_c(1 )  =  X(1 );  %  initial  waypoint  is  the  vehicle  starting  point 

PrevY_Way_c(1)  =  Y(1); 

%To  determine  LOS  angle 

theta  =  atan((Y(1)-Y_Way_c(1))/(X(1)-X_Way_c(1)))*180/pi;  %LOS  angle  between  initial  vehicle 

position  and  cage  position 

%  Ensure  correct  LOS  angle  between  initial  vehicle  position  and  cage  position  for  different 
conditions 

% - 

if  ((X(1)-X_Way_c(1))<0)  &  ((Y(1)-Y_Way_c(1))<0) 
theta=180+theta; 
end 

if  ((X(1)-X_Way_c(1))>0)  &  ((Y(1)-Y_Way_c(1))>0) 
theta=theta; 
end 

if  ((X(1)-X_Way_c(1))<0)  &  ((Y(1)-Y_Way_c(1))>0) 
theta=180+theta; 
end 

if  ((X(1)-X_Way_c(1))>0)  &  ((Y(1)-Y_Way_c(1))<0) 
theta=360+theta; 
end 

if  ((X(1  )-X_Way_c(1  ))==0)  &  ((Y(1  )-Y_Way_c(1  ))<0) 
theta=360+theta; 
end 

if  ((X(1  )-X_Way_c(1  ))==0)  &  ((Y(1  )-Y_Way_c(1  ))>0) 
theta=360+theta; 
end 

if  ((X(1)-X_Way_c(1))>0)  &  ((Y(1)-Y_Way_c(1))==0) 
theta=360+theta; 
end 

if  ((X(1)-X_Way_c(1))<0)  &  ((Y(1)-Y_Way_c(1))==0) 
theta=180+theta; 
end 

% - 

%LOS  distance  from  initial  vehicle  position  to  cage  position 
Length  =  sqrt((X_Way_c(1)-X(1))''2+(Y_Way_c(1)-Y(1))''2); 
initial_Length  =  Length; 

%  To  determine  the  cage  orientation 

%X  co-ordinate  of  the  initial  vehicle  position  from  the  cage  position  -  for  plotting 
X_LOS  =  X_Way_c(1)+initial_Length*cos(theta  *DegRad) 

%Y  co-ordinate  of  the  initial  vehicle  position  from  the  cage  position  -  for  plotting 
Y_LOS  =  Y_Way_c(1)+initial_Length*sin(theta  *DegRad) 

)%  X  co-ordinate  of  centre  of  cone  which  is  of  distance  "Length"  away  from  cage  position 
X_cage  =  X_Way_c(1)+initial_Length*cos(initial_cage_psi  *DegRad 
%  Y  co-ordinate  of  centre  of  cone  which  is  of  distance  "Length"  away  from  cage  position 
Y_cage  =  Y_Way_c(1)+initial_Length*sin(initial_cage_psi  *DegRad) 

%  X  co-ordinate  of  left  limit  of  cone  of  the  cage  (2D) 

L_cone_x  =  X_Way_c(1)+cage_length*cos((initial_cage_psi  +30)*DegRad); 
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%  Y  co-ordinate  of  left  limit  of  cone  of  the  cage  (2D) 

L_cone_y  =  Y_Way_c(1)+cage_length*sin((initial_cage_psi  +30)*DegRad); 

%  X  co-ordinate  of  right  limit  of  cone  of  the  cage  (2D) 

R_cone_x  =  X_Way_c(1)+cage_length*cos((initial_cage_psi  -30)*DegRad); 

%  Y  co-ordinate  of  right  limit  of  cone  of  the  cage  (2D) 

R_cone_y  =  Y_Way_c(1)+cage_length*sin((initial_cage_psi  -30)*DegRad); 

%To  allow  plotting  when  part  of  cone  co-ordinate  equals  cage  position  co-ordinates 

% - 

if  ((L_cone_x-X_Way_c(1))  ==  0) 
xjint  =  1 ; 
else 

xjint  =  L_cone_x-X_Way_c(1); 
end 

if  ((L_cone_y-Y_Way_c(1))  ==  0) 
yjint  =  1 ; 
else 

yjint  =  L_cone_y-Y_Way_c(1); 
end 

if  ((R_cone_x-X_Way_c(1))  ==  0) 
x_rint  =  1 ; 
else 

x  rint  =  R_cone_x-X  \A/ay_c(1); 
end 

if  ((R_cone_y-Y_Way_c(1))  ==  0) 
y_rint  =1; 
else 

y_rint  =  R_cone_y-Y_Way_c(1); 
end 

0/„ - 

%  angle  between  LOS  angle  and  cage  initial  orientation  angle 
Diff_angle  =  theta-initial_cage_psi; 

%  DeWrap  Dlff_angle  to  within  +/-  pi;  Normalized  to  Lie  between  +/- 180 
%  degrees 

while(abs(Diff_angle)  >  180) 

Diff_angle  =  Diff_angle  -  sign(Diff_angle)*360; 
end; 

%  Compute  the  number  of  waypoints  and  their  positions 
[X  \A/ay_c,Y  \A/ay_c,SurfPhase,W_R, No  tracks]  = 

waypoint2(X_cage,Y_cage,XcageJnitial,YcageJnitial,Diff_angle,initial_cage_psi, Length) 

%  Total  Track  Length  between  initial  waypoint  and  waypoint  (1) 

SegLen(l)  =  sqrt((X_Way_c(1)-PrevX_Way_c(1))''2+(Y_Way_c(1)... 

-PrevY_Way_c(1  ))''2); 

%  T rack  Angle  of  first  track 

psijrack(1 )  =  atan2(Y_Way_c(1  )-PrevY_Way_c(1  ),X_Way_c(1  )-PrevX_Way_c(1 )); 

%  Track  Length  and  track  angle  for  subsequent  tracks 
for  j=2:No  tracks, 

SegLenQ)  =  sqrt((X_Way_c(j)-X_Way_c(j-1))''2+(Y_Way_c(j)-... 

Y_Way_cG-1))''2); 

psijrack(j)  =  atan2(Y_Way_c(j)-Y_Way_c(j-1  ),X_Way_c(j)-X_Way_c(j-1 )); 
end; 
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j=i; 

for  i  =  1:length(t)-1, 

Depth_com(i)  =WayPointVertDist_com; 
time  =  time  +  dt; 


%  Cage  position  updates  to  the  vehicle  at  every  three  seconds 
if  (mod(time,3)  ==  0) 

%  Set  initial  conditions 

F1; 

time 

%if  time  <  50 

initial_cage_psi  =  180+3*time;  %  Initial  cage  yaw  angle 

%else 

%initial_cage_psi  =  180-2*time; 

%end 


%  Set  start  position 

X_Way_c(1)  =  Xcagejnitial;  %  Reset  waypoint  #1  as  the  cage  position 

Y_Way_c(1)  =  Ycagejnitial; 

W_R(1)  =  1.0;  %  Sets  initial  Watch  Radius 

SurfPhase(l)  =  0; 

Notracks  =1; 

Depth_com(1)  =  5.0; 

WayPointVertDistcom  =  5.0; 

SURFACE_TIMER_ACTIVE  =  FALSE; 

PrevX_Way_c(1)  =  X(i);  %  Set  initial  waypoint  to  current  vehicle  position 

PrevY_Way_c(1)  =  Y(i); 

%To  determine  LOS  angle 

%LOS  angle  from  current  position  of  vehicle  to  waypoint  #1(also  cage  position) 
theta  =  atan((Y(i)-Y_Way_c(1))/(X(i)-X_Way_c(1)))*1 80/pi; 

%  Ensure  correct  LOS  angle  between  current  vehicle  position  and  waypoint  #1(also  cage 
position)  for  different  conditions 

% - 

if  ((X(i)-X_Way_c(1))<0)  &  ((Y(i)-Y_Way_c(1))<0) 
theta=180+theta; 
end 

if  ((X(i)-X_Way_c(1))>0)  &  ((Y(i)-Y_Way_c(1))>0) 
theta=theta; 
end 

if  ((X(i)-X_Way_c(1))<0)  &  ((Y(i)-Y_Way_c(1))>0) 
theta=180+theta; 
end 

if  ((X(i)-X_Way_c(1))>0)  &  ((Y(i)-Y_Way_c(1  ))<0) 
theta=360+theta; 
end 

if  ((X(i)-X_Way_c(1))==0)  &  ((Y(i)-Y_Way_c(1))<0) 
theta=360+theta; 
end 

if  ((X(i)-X_Way_c(1))==0)  &  ((Y(i)-Y_Way_c(1))>0) 
theta=360+theta; 
end 
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if  ((X(i)-X_Way_c(1))>0)  &  ((Y(i)-Y_Way_c(1))==0) 
theta=360+theta; 
end 

if  ((X(i)-X_Way_c(1))<0)  &  ((Y(i)-Y_Way_c(1  ))==0) 
theta=180+theta; 
end 

% - 

%LOS  distance  from  current  vehicle  position  to  cage  position 
Length  =  sqrt((X_Way_c(1)-X(i))''2+(Y_Way_c(1)-Y(i))''2); 

%  To  determine  the  cage  orientation 

%X  co-ordinate  of  the  current  vehicle  position  from  the  cage  position  -  for  plotting 
X  LOS  =  X_Way_c(1)+Length*cos(theta  *DegRad); 

%Y  co-ordinate  of  the  current  vehicle  position  from  the  cage  position  -  for  plotting 
Y_LOS  =  Y_Way_c(1)+Length*sin(theta  *DegRad); 

X_cage  =  X_Way_c(1)+Length*cos(initial_cage_psi  *DegRad); 

Y_cage  =  Y_Way_c(1)+Length*sin(initial_cage_psi  *DegRad); 

%  X  co-ordinate  of  left  limit  of  cone  of  the  cage  (2D) 

L_cone_x  =  X_Way_c(1)+cage_length*cos((initial_cage_psi  +30)*DegRad); 

%  Y  co-ordinate  of  left  limit  of  cone  of  the  cage  (2D) 

L_cone_y  =  Y_Way_c(1)+cage_length*sin((initial_cage_psi  +30)*DegRad); 

%  X  co-ordinate  of  right  limit  of  cone  of  the  cage  (2D) 

R_cone_x  =  X_Way_c(1)+cage_length*cos((initial_cage_psi  -30)*DegRad); 

%  Y  co-ordinate  of  right  limit  of  cone  of  the  cage  (2D) 

R_cone_y  =  Y_Way_c(1)+cage_length*sin((initial_cage_psi  -30)*DegRad); 

%To  allow  plotting  when  part  of  cone  co-ordinate  equals  cage  position  co-ordinates 

% - 

if  ((L_cone_x-X_Way_c(1 ))  ==  0) 
xjint  =  1; 
else 

xjint  =  L_cone_x-X_Way_c(1); 
end 

if  ((L_cone_y-Y_Way_c(1 ))  ==  0) 
yjint  =  1; 
else 

yjint  =  L_cone_y-Y_Way_c(1); 
end 

if  ((R_cone_x-X_Way_c(1))  ==  0) 
x_rint  =  1 ; 
else 

x  rint  =  R_cone_x-X  \A/ay_c(1); 
end 

if  ((R_cone_y-Y_Way_c(1))  ==  0) 
y_rint  =1; 
else 

y_rint  =  R_cone_y-Y_Way_c(1); 
end 

% - 


%  angle  between  LOS  angle  and  cage  initial  orientation  angle 
Diff_angle  =  theta-initial_cage_psi; 

%  DeWrap  Diff_angle  to  within  +/-  pi;  Normalized  to  Lie  between  +/- 180 
%  degrees 

while(abs(Diff_angle)  >  180) 
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Diff_angle  =  Diff_angle  -  sign(Diff_angle)*360; 
end; 

%  Compute  the  number  of  waypoints  and  their  positions 
[X_Way_c,Y_Way_c,SurfPhase,W_R,No_tracks]  = 
waypoint2(X_cage,Y_cage,Xcage_initial,Ycage_initial,Diff_angle,initial_cage_psi,  Length); 

%  Total  Track  Length  between  initial  waypoint  and  waypoint  (1) 

SegLen(1)  =  sqrt((X_Way_c(1)-PrevX_Way_c(1))''2+(Y_Way_c(1)... 

-PrevY_Way_c(1  ))''2); 

%  T rack  Angle  of  first  track 

psi_track(1 )  =  atan2(Y_Way_c(1  )-PrevY_Way_c(1  ),X_Way_c(1  )-PrevX_Way_c(1 )); 

%  Track  Length  and  track  angle  for  subsequent  tracks 
for  j=2:No_tracks, 

SegLenQ)  =  sqrt((X_Way_c(j)-X_Way_c(j-1))''2+(Y_Way_c(j)-... 

Y_Way_cG-1))''2); 

psi_trackG)  =  atan2(Y_Way_cG)-Y_Way_cG-1  ),X_Way_cG)-X_Way_cG-1 )); 
end; 
j=i; 
end 


%  Difference  between  current  vehicle  position  &  the  next  waypoint  Eq(13) 

X_Way_Error(i)  =  X_Way_cG)  -  X(i); 

Y_Way_Error(i)  =  Y_Way_cG)  -  Y(i); 

%  DeWrap  psi  to  within  +/-  2.0*pi;  Makes  Heading  Angle  to  lie  between 
%  0-360  degrees 

psi_cont(i)  =  psi(i); 
while(abs(psi_cont(i))  >  2.0*pi) 
psi_cont(i)  =  psi_cont(i)  -  sign(psi_cont(i))*2.0*pi; 
end; 

%  Cross  Track  Heading  Error  Eq(12) 

psi_errorCTE(i)  =  psi_cont(i)  -  psi_trackG); 

%  DeWrap  psi_error  to  within  +/-  pi;  Normalized  to  Lie  between  +/- 180 
%  degrees 

while(abs(psi_errorCTE(i))  >  pi) 

psi_errorCTE(i)  =  psi_errorCTE(i)  -  sign(psi_errorCTE(i))*2.0*pi; 
end; 

%  **  Always  Calculate  this  (What  is  This?) 

Beta  =  v(i)/U; 

%  Beta  =  0.0; 

cpsi_e  =  cos(psi_errorCTE(i)+Beta); 
spsi_e  =  sin(psi_errorCTE(i)+Beta); 

%  Distance  to  the  ith  way  point  projected  to  the  track  line  S(t)i  - 
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%  Eq  (14) 

s(i)  =  [X_Way_Error(i),Y_Way_Error(i)]*[(X_Way_cG)-... 

Pre  vX_Way_cG )) ,  (Y_Way_cG  )-PrevY_Way_cG ))]' ; 

%  s  is  distance  to  go  projected  to  track  line(goes  from  0-100%L)  -  Eq  (14) 

s(i)  =  s(i)/SegLenG); 

Ratio=(1 .0-s(i)/SegLenG))*1 00.0;  %  Ranges  from  0-1 00%  of  SegLen 

%  Radial  distance  to  go  to  next  WP 

ss(i)  =  sqrt(X_Way_Error(i)''2  +  Y_Way_Error(i)''2); 

%  dp  is  angle  between  line  of  sight  and  current  track  line  -  Eq  (16) 

dp(i)  =  atan2(  (Y_Way_cG)-PrevY_Way_cG)),(X_Way_cG)-... 

PrevX_Way_cG)) )-  atan2(  Y_Way_Error(i),X_Way_Error(i) ); 
if(dp(i)  >  pi), 

dp(i)  =  dp(i)-2.0*pi; 
end; 

%  Cross  Track  Error  Definition  -  Eq  (15) 
cte(i)  =  s(i)*sin(dp(i)); 

%  If  the  magnitude  of  the  CTE  Heading  exceeds  40  degrees,  a  LOS  Controller 
%  is  used. 

if(  abs(psi_errorCTE(i))  >=  40.0*pi/180.0  |  s(i)  <  0.0  ), 

LOS(i)  =  1; 

psi_comLOS  =  atan2(Y_Way_Error(i),X_Way_Error(i));  %  Eq  (22) 
psi_errorLOS(i)  =  psi_comLOS  -  psi_cont(i);  %  Eq  (23) 

%  LOS  Error 

if(abs(psi_errorLOS(i))  >  pi), 

psi_errorLOS(i)  =  psi_errorLOS(i)  -  2.0*pi*psi_errorLOS(i)... 
/abs(psi_errorLOS(i)); 

end; 

%  Eq  (8) 

Sigma_FlightHeading  =  0.9499*(r_com  -  r(i))  +  0.1701*psi_errorLOS(i); 

%  Eq  (9) 

dr(i)  =  -1.5435*(  2.5394*r(i)+  Eta_FlightHeading*tanh... 
(Sigma_FlightHeading/Phi_FlightHeading)); 

else 

%  Use  CTE  Controller  if  CTE  Heading  is  less  than  40  degrees 
LOS(i)  =  0; 

if(cpsi_e  ~=  0.0),  %  Trap  Div.  by  Zero  ! 

%  SMC  Soln 
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%  Sliding  Surface  -  Eq  (20) 


Sigma(i)  =  U*rRM(i)*cpsi_e  +  Lam1*U*spsi_e  +  3.28*Lam2*cte(i); 
%  Rudder  Input -Eq  (21) 

dr(i)  =  (1.0/(U*b*cpsi_e))*(-U*a*rRM(i)*cpsi_e  +  U*rRM(i)''2*... 
spsi_e  -  Lam1*U*rRM(i)*cpsi_e  -  Lam2*U*spsi_e  -  Eta_CTE* ... 
(Sigma(i)/Phi_CTE)); 

else 

dr(i)  =  dr(i-1); 
end; 

end;  %  End  of  GTE  Controller 


if(SurfPhaseG)  ==  TRUE) 
if(SURFACE_TIMER_ACTIVE  ==  FALSE) 
if(Ratio  >  40.0) 

%  Start  a  Timer 

SURFACE_TIMER_ACTIVE  =  TRUE; 
Depth_com(i)  =  0.0; 

SurfaceWait  =  SurfaceTimeQ)  +  t(i); 
SurfaceWait 
end; 
end; 
end; 

if(SURFACE_TIMER_ACTIVE  ==  TRUE) 
if(t(i)  >=  SurfaceWait) 
SURFACE_TIMER_ACTIVE  =  FALSE; 
Depth_com(i)  =  WayPointVertDist_com(j); 
SurfPhaseQ)  =  0; 
else 

Depth_com(i)  =  0.0; 

end; 

end; 

if(abs(dr(i))  >  0.4) 
dr(i)  =  0.4*sign(dr(i)); 
end; 


x_dot(:,i+1)  =  [  A(1,1)*v(i)  +  A(1,2)*r(i)  +  B(1)*dr(i); 
A(2,1)*v(i)  +  A(2,2)*r(i)  +  B(2)*dr(i); 
r(i)]; 

x(:,i+1)  =  x(:,i)+dt*x_dot(:,i); 
v(i+1)  =x(1,i+1)/12; 
r(i+1)  =x(2,i+1); 
psi(i+1)  =  x(3,i+1); 
rRM(i+1)  =  r(i+1); 


Uc  =  0.0; 
Vc  =  0.0; 


%  Kinematics 
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%X(i+1 )  =  X(i)  +  (Uc  +  (U)*cos(psi(i))  -  v(i)*sin(psi(i)))*dt; 

%Y(i+1)  =  Y(i)  +  (Vc  +  (U)*sin(psi(i))  +  v(i)*cos(psi(i)))*dt; 

X(i+1)  =  X(i)  +  (Uc  +  (U/3.28)*cos(psi(i))  -  v(i)/3.28*sin(psi(i))  )*dt; 

Y(i+1)  =  Y(i)  +  (Vc  +  (U/3.28)*sin(psi(i))  +  v(i)/3.28*cos(psi(i))  )*dt; 

************************************************************************** 

%  Check  if  Aries  falls  within  the  cage 

X_tip(i)  =  X(i)  +  xcg/3.28*cos(psi(i));  %  X  co-ordinate  of  tip  of  the  vehicle's  nose 

Y_tip(i)  =  Y(i)  +  xcg/3.28*sin(psi(i));  %  Y  co-ordinate  of  tip  of  the  vehicle's  nose 

%X_centre_cage  =  Xcage_initiai+  cage_length*cos(initial_cage_psi  *pi/180); 
%Y_centre_cage  =  Ycage_initial+  cage_length*sin(initial_cage_psi  *pi/180); 

%FOV  extended  to  the  initial  vehicle  start  point's  distance 
L_cone_xFOV  =  Xcage_initial+initial_Length*cos(mod(initial_cage_psi  +30,360)*DegRad); 
L_cone_yFOV  =  Ycage_initial+initial_Length*sin(mod(initial_cage_psi  +30,360)*DegRad); 
R_cone_xFOV  =  Xcage_initial+initial_Length*cos(mod(initial_cage_psi  -30,360)*DegRad); 
R_cone_yFOV  =  Ycage_initial+initial_Length*sin(mod(initial_cage_psi  -30,360)*DegRad); 
Xcurrent  =  X_tip(i); 

Ycurrent  =  Y_tip(i); 
psicurrent  =  psi(i); 

%Angle  of  vehicle  tip  w.r.t  cage  position 

vehicle_cage_angle  =  atan2(Y_tip(i)-Ycage_initial,X_tip(i)-Xcage_initial)*1 80/pi; 

%  DeWrap  vehicle_cage_angle  to  within  +/-  pi;  Normalized  to  Lie  between  +/- 180 
%  degrees 

while(abs(vehicle_cage_angle)  >  180) 

vehicle_cage_angle  =  vehicie_cage_angle  -  sign(vehicle_cage_angle)*360; 
end; 

vehicle_cage_angle  =  mod(vehicle_cage_angle,360); 

Delta_angle  =  vehicle_cage_angle-initial_cage_psi;  %in  degree 
LengthLOS  =  sqrt((Y_tip(i)-Ycage_initial)''2+(X_tip(i)-Xcage_initial)''2); 

LengthLOSreal  =  sqrt((Y(i)-Ycage_initial)''2+(X(i)-Xcage_initial)''2); 

maxdistance  =  cage_length*cos(30*DegRad) 
currentdistance  =  LengthLOS*cos(Delta_angle*DegRad) 
currentdistancereal  =  LengthLOSrearcos(Delta_angle*DegRad) 
mindistance  =  cage_length*cos(30*DegRad)-0.6 

minangle  =  mod(initial_cage_psi-30,360) 
vehicle_cage_angle 

maxangle  =  mod(initial_cage_psi+30,360) 

%Set  acceptance  criteria  tolerances  -0.6ft  from  cage  cone  length 
%L_cone_xmax  =  Xcage_initial+(cage_length+(0.3/cos(30*pl/1 80)))*cos((initlal_cage_psi 
+30)*pl/180); 

%L_cone_ymax  =  Ycage_initlal+(cage_length+(0.3/cos(30*pl/1 80)))*sin((initial_cage_psi 
+30)*pl/180); 

%R_cone_xmax  =  Xcage_initial+(cage_length+(0.3/cos(30*pl/180)))*cos((initial_cage_psi  - 
30)*pi/180); 

%R_cone_ymax  =  Ycage_initial+(cage_length+(0.3/cos(30*pl/180)))*sin((initial_cage_psi  - 
30)*pl/180); 

L_cone_xmin  =  Xcage_initial+(cage_length-(0.6/cos(30*DegRad)))*cos(mod(initial_cage_psi 
+30,360)*DegRad); 
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L_cone_ymin  =  Ycage_initial+(cage_length-(0.6/cos(30*DegRad)))*sin(mod(initial_cage_psi 
+30,360)*DegRad); 

R_cone_xmin  =  Xcage_initial+(cage_length-(0.6/cos(30*DegRad)))*cos(mod(initial_cage_psi  - 
30,360)*DegRad); 

R_cone_ymin  =  Ycage_initial+(cage_length-(0.6/cos(30*DegRad)))*sin(mod(initial_cage_psi  - 
30,360)*DegRad); 

status  =0; 

if  (mod(initial_cage_psi,360)  <  30)|  (mod(initial_cage_psi,360)  >330) 
if  (vehicle_cage_angle>=mod(initial_cage_psi-30,360))| 
(vehicle_cage_angle<=mod(initial_cage_psi+30,360)) 
disp(sprintf('Vehicle  is  within  the  FOV  of  cage')); 
status  =1; 
end 
end 

if  (vehicle_cage_angle>=mod(initial_cage_psi-30,360))  & 
(vehicle_cage_angle<=mod(initial_cage_psi+30,360)) 
disp(sprintf('Vehicle  is  within  the  FOV  of  cage')); 
status  =1; 
end 

if  (status  ==  1) 

Rconex 

Rconexmin 

if  (R  cone  x  ==  L  cone  x) 

if  (abs(vehicle_cage_angle)  <=  90)|(abs(vehicle_cage_angle)  >=  270) 
if  (X_tip(i)  <=  L_cone_x)  &  (X_tip(i)  >=  L_cone_xmin) 
disp(sprintf('Vehicle  is  about  to  dock  into  the  cage')); 
if  ((Y_tip(i)  >  L_cone_y)  |  (Y_tip(i)  <  R_cone_y)) 
disp(sprintf('Vehicle  misses  the  cage')); 
else 

disp(sprintf('Vehicle  docks  into  the  cage')); 
end 
end 
end 

if  (abs(vehicle_cage_angle)  >  90)&(abs(vehicle_cage_angle)  <  270) 
if  (X_tip(i)  >=  L_cone_x)  &  (X_tip(i)  <=  L_cone_xmin) 
disp(sprintf('Vehicle  is  about  to  dock  into  the  cage')); 
if  ((Y_tip(i)  >  R_cone_y)|(Y_tip(i)  <  L_cone_y)) 
disp(sprintf('Vehicle  misses  the  cage')); 
else 

disp(sprintf('Vehicle  docks  into  the  cage')); 
end 
end 
end 

end 


if  (R  cone  x  ~=  L  cone  x) 

if  (LengthLOS*cos(Delta_angle*DegRad)  <=  cage_length*cos(30*DegRad))  &  ... 
(LengthLOS*cos(Delta_angle*DegRad)  >  cage_length*cos(30*DegRad)-0.6) 
disp(sprintf('Vehicle  is  about  to  dock  into  the  cage')); 
disp(sprintf('Vehicle  docks  into  the  cage')); 
end 
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end 

else 


disp(sprintf('Vehicle  is  NOT  within  the  FOV  of  cage')); 
end 

status  =0; 


%  Check  to  See  if  we  are  Within  the  Watch_Radius 
test  =  j; 

test1  =No_tracks; 

test2  =  sqrt(X_Way_Error(i)''2.0  +  Y_Way_Error(i)''2.0); 
tests  =X_Way_Error(i); 
test4  =  Y_Way_Error(i); 
tests  =  s(i); 

if(sqrt(X_Way_Error(i)''2.0  +  Y_Way_Error(i)''2.0)  <=  W_RG)  |  s(i)  <  0.0) 
disp(sprintf('WayPoint  %d  Reached',))); 
if(j==No_tracks) 

X_Way_Error(i); 

Y_Way_Error(i); 

break; 

end; 

PrevX_Way_c(j+1)  =  X_Way_c(j); 

PrevY_Way_c(j+1)  =  Y_Way_c(j); 

Fj+1; 

end; 

end; 

************************************************************************** 


%dr(i+1)  =  dr(i); 
%cte(i+1)  =  cte(i); 
%s(i+1)  =  s(i); 


%  Planned  Track 
forz  =1:No_tracks 

plot([X_Way_c(z)  PrevX_Way_c(z)],[Y_Way_c(z)  PrevY_Way_c(z)],'r'); 
hold  on 

plot(Xcage_initial,Ycage_initial,'o'); 
hold  on 

plot(X_Way_c(z),Y_Way_c(z),'*'); 
hold  on 
end 

%text(0,0,['theta=  '  int2str(theta)]) 

%hold  on 

plot(X_LOS,Y_LOS,'s') 

hold  on 

plot(X,Y); 

hold  on 

plot(Xcage_initial:x_rint:R_cone_x,Ycage_initial:y_rint:R_cone_y,'c') 
hold  on 

plot(Xcage_initial:x_lint:L_cone_x,Ycage_initial:y_lint:L_cone_y,'c') 
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text(Xcage_initial+10,Ycage_initial+40,['Xcage  =  '  int2str(Xcage_initial)]); 
text(Xcage_initial+10,Ycage_initial+20,['Ycage  =  '  int2str(Ycage_initial)]); 
text(Xcage_initial+10,Ycage_initial,['X  =  '  int2str(X(i))]); 
text(Xcage_initial+10,Ycage_initial-20,['Y  =  '  int2str(Y(i))]); 
hold  on 

if  (Xcage_initial>  X(1 ))  &  (Ycage_initial>  Y(1 )) 

Axis([X(1)-100  Xcage_initial+100  Y(1)-100  Ycage_initial+100]); 
end 

if  (Xcage_initial<  X(1))  &  (Ycage_initial<  Y(1)) 

Axis([Xcage_initial-1 00  X(1 )+ 1 00  Ycage_initial-1 00  Y(1 )+ 1 00]); 
end 

if  (Xcage_initial<  X(1 ))  &  (Ycage_initial>  Y(1 )) 

Axis([Xcage_initial-1 00  X(1 )+ 1 00  Y(1  )-1 00  Ycage_initial+1 00]); 
end 

if  (Xcage_initial>  X(1 ))  &  (Ycage_initial<  Y(1 )) 

Axis([X(1  )-1 00  Xcage_initial+ 1 00  Ycage_initial-1 00  Y(1  )+1 00]); 

end 
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APPENDIX  B:  MATLAB  FILE  WAYP0INT2.M 


function  [X_Way_c,Y_Way_c,SurfPhase,W_R,No_tracks]  = 

waypoint(X_cage,Y_cage,Xcage_initial,Ycage_initial,Diff_angle,initial_cage_psi,  Length) 
SurfPhase  =0.0; 
if  (Diff_angle>=30) 

if  ((Diff_angle>=30)  &  (Diff_angle<=60)) 

X_Way_c(1)  =  Xcage; 

Y_Way_c(1)  =  Ycage; 

X_Way_c(2)  =  Xcagejnitial; 

Y_Way_c(2)  =  Ycagejnitial; 

Notracks  =2; 

W_R(2)  =5; 

SurfPhase(2)  =  0; 
end 

if  ((Diff_angle>60)  &  (Diff_angle<=90)) 

Notracks  =3; 
end 

if  ((Diff_angle>90)  &  (Diff_angle<=120)) 

Notracks  =4; 
end 

if  ((Diff_angle>120)  &  (Diff_angle<=150)) 

Notracks  =5; 
end 

if  ((Diff_angle>150)  &  (Diff_angle<=180)) 

Notracks  =6; 
end 

for  z  =  2:No_tracks-1 
W_R(z)=10.0-z; 
end 

W_R(No_tracks)  =1 .0; 
for  z  =  2:No_tracks 
SurfPhase(z)  =  0; 
end 

for  z  =1  :No_tracks-1 

if  ((No_tracks>=3)  &  (z<=No_tracks-3)) 

X_cage  =  Xcage_initial+Length*cos((initial_cage_psi+(No_tracks*30-30)-z*30)  *pi/180); 
Y_cage  =  Ycage_initial+Length*sin((initial_cage_psi+(No_tracks*30-30)-z*30)  *pi/180); 
else 

%X_cage  =  Xcage_initial+Length/2*cos((initial_cage_psi+(No_tracks*30-30)-z*30) 
*pi/180); 

%Y_cage  =  Ycage_initial+Length/2*sin((initial_cage_psi+(No_tracks*30-30)-z*30) 
*pi/180); 

X_cage  =  Xcage_initial+Length*cos((initial_cage_psi+(No_tracks*30-30)-z*30)  *pi/180); 
Y_cage  =  Ycage_initial+Length*sin((initial_cage_psi+(No_tracks*30-30)-z*30)  *pi/180); 
end 

X_Way_c(z)  =  Xcage; 

Y_Way_c(z)  =  Ycage; 

Length  =  sqrt((Xcage_initial-X_Way_c(z))''2+(Ycage_initial-Y_Way_c(z))''2); 
end 

X_Way_c(No_tracks)  =  Xcagejnitial; 
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Y_Way_c(No_tracks)  =  Ycagejnitial; 
end 

if  (Diff_angle<=-30) 

if  ((Diff_angle<=-30)  &  (Diff_angle>=-60)) 

X_Way_c(1)  =  Xcage; 

Y_Way_c(1)  =  Ycage; 

X_Way_c(2)  =  Xcagejnitial; 

Y_Way_c(2)  =  Ycagejnitial; 

No  tracks  -2] 

W_R(2)  =1 ; 

SurfPhase(2)  =  0; 
end 

if  ((Diff_angle<-60)  &  (Diff_angle>=-90)) 

No  tracks  =3; 
end 

if  ((Diff_angle<-90)  &  (Diff_angle>=-120)) 

No  tracks  =4; 
end 

if  ((Diff_angle<-120)  &  (Diff_angle>=-150)) 

No  tracks  =5; 
end 

if  ((Diff_angle<-150)  &  (Diff_angle>-180)) 

No  tracks  =6; 
end 

for  z  =  2:No  tracks-1 
W_R(z)=10.0-z; 
end 

W_R(NoJracks)=1.0; 
for  z  =  2:No  tracks 
SurfPhase(z)  =  0; 
end 

for  z  =1  :No  tracks-1 

if  ((No  tracks>=3)  &  (z<=No  tracks-3)) 

X_cage  =  XcageJnitial+Length*cos((initial_cage_psi-(NoJracks*30-30)+z*30)*pi/1 80); 
Y_cage  =  YcageJnitial+Length*sin((initial_cage_psi-(NoJracks*30-30)+z*30)*pi/180); 
else 

X_cage  =  XcageJnitial+Length*cos((initlal_cage_psi-(NoJracks*30-30)+z*30)*pi/1 80); 
Y_cage  =  YcageJnitial+Length*sin((initial_cage_psi-(NoJracks*30-30)+z*30)*pl/180); 
%X_cage  =  XcageJnitial+Length/2*cos((initial_cage_psi-(NoJracks*30-30)+z*30)*pi/180); 
%Y_cage  =  YcageJnitial+Length/2*sin((initial_cage_psi-(NoJracks*30-30)+z*30)*pi/1 80); 

end 

X_Way_c(z)  =  Xcage; 

Y_Way_c(z)  =  Ycage; 

Length  =  sqrt((XcageJnitial-X_Way_c(z))''2+(Ycage_initial-Y_Way_c(z))''2); 
end 

X_Way_c(No  tracks)  =  Xcagejnitial; 

Y_Way_c(NoJracks)  =  Ycagejnitial; 

end 

if  ((Diff_angle>-30)  &  (Diff_angle<30)) 

No  tracks  =  1 ; 
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X_Way_c(No_tracks)  =  Xcagejnitial; 
Y_Way_c(No_tracks)  =  Ycagejnitial; 
end 

if  (No  tracks  <  2) 

W_R(1)  =1.0; 
else 

W_R(1)  =10.0; 
end 
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APPENDIX  C:  MATLAB  FILE  DELAY2DEG.M 


clear 

cic 

num=0; 

TRUE  =  1; 

FALSE  =  0; 

direction=0;  %initialise  the  direction  of  cage  motion 

check_condition  =0; 

DegRad  =  pi/180; 

%  Length  from  tip  of  the  nose  to  CG  position  in  X-co-ordinate  (assuming  xcg=0.5xLength  of  Aries 
xcg  =  5.33; 

%  Length  of  Aries  =  128  inches  =  10.67ft.  xcg=1 0.67/2  =  5.33ft 
%  Jay  Johnson  Model 

m  =  222.26; 

U  =  1 .9*3.28;  %  Forward  Speed 

Yv  =-68.16; 

Yr  =  406.3; 

Ydr  =  70.0; 

Nv  =-10.89; 

Nr  =  -88.34; 

Ndr  =  -35.47; 

MY  =  456.76; 

IN  =  215; 

M  =  diag([MY,IN,1]); 

AA  =  [Yv,Yr-(m*U),0;Nv,Nr,0;0,1 ,0]; 

BB  =  [Ydr;Ndr;0]; 

A  =  inv(M)*AA; 

B  =  inv(M)*BB; 

%  Below  are  in  British  Units  for  CTE  Sliding  Mode 

Lami  =2.0; 

Lam2  =  1.0; 

cagejength  =1 ;  %  length  cone  of  the  cage  in  meter 

Eta_FlightHeading  =  1.0; 

Phi_FlightHeading  =  0.5; 

%  Below  for  tanh 

Eta_CTE  =  0.1; 

Eta_CTE_Min  =  1 .0; 

Phi_CTE  =  0.5; 

Uc  =  []; 

Vc  =  []; 


Sigma  =  []; 
Depth_com  =  []; 
dr=[]; 
a  =  -.3; 
b  =  (9/24)*a; 
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%  delta  T  per  iteration 


dt  =  0.05; 
t  =  [0:dt:550]'; 
time  =  0; 
room  =  0.0; 

SurfaceTime  =  25; 

%  Set  initial  conditions 

v(1)  =0.0;  %  Initial  Side  Slip  Velocity 

r(1)  =0.0;  %  Initial  Yaw 

rRM(1)  =  r(1); 

Xpos  =  input('lnitial  X  position  of  vehicle  in  meters  (Default:100.0) :');  %  Initial  Position  in  meters 
if  isempty(Xpos), 

X(1)=100.0; 

else 

X(1)=Xpos; 

end 

Ypos  =  input('lnitial  Y  position  of  vehicle  in  meters  (Default:0.0)  :');  %  Initial  Position  in  meters 
if  isempty(Ypos), 

Y(1)=0.0; 

else 

Y(1)=Ypos; 

end 

psideg  =input('lnitial  vehicle  orientation  in  degree  (Default:0.0) :');  %  Initial  vehicle  orientation 
if  isempty(psideg), 
psi(1)=0.0; 
else 

psi(1)=psideg; 

end 

psi(1)  =  psi(1)*DegRad;  %  Initial  Heading  of  ARIES 

W_R(1)  =  1 .0;  %  Sets  initial  Watch  Radius 

x(:,1)  =  [v(1);r(1);psi(1)]; 

SurfPhase(l)  =  0; 

Notracks  =1 ; 

Depth_com(1)  =  5.0; 

WayPointVertDistcom  =  5.0; 

SURFACE_TIMER_ACTIVE  =  FALSE; 

Xcagejnitial  =  input('lnitial  X  position  of  cage  in  meters  (Default:200.0)  :');  %  Initial  Position  of 
cage  in  meters 
if  isempty(Xcage_initial), 

Xcage_initial=200.0; 

end 

Ycagejnitial  =  input('lnitial  Y  position  of  cage  in  meters  (Default:0.0)  :');  %  Initial  Position  of  cage 
in  meters 

if  isempty(Ycage_initial), 

Ycage_initial=0.0; 

end 

initial_cage_psi  =  input('lnitial  cage  orientation  in  degree  from  0-360  (Default:! 80.0)  :');  %  Initial 
cage  orientation 
if  isempty(initial_cage_psi), 


72 


initial_cage_psi=  180.0; 
initial_cage_psi=mod(initial_cage_psi,360); 
end 


%  Set  start  position 

%  current  waypoint  is  the  cage  position 
X_Way_c(1)  =  Xcagejnitial; 

Y_Way_c(1)  =  Yea  gejnitial; 

%  initial  waypoint  is  the  vehicle  starting  point 
PrevX_Way_c(1)  =  X(1); 

PrevY_Way_c(1)  =  Y(1); 

%  initialise  the  cage  angular  sensor 
cage_psi_old  =  initial_cage_psi; 

%To  determine  LOS  angle 

%LOS  angle  between  initial  vehicle  position  and  cage  position 
theta  =  atan((Y(1)-Y_Way_c(1))/(X(1)-X_Way_c(1)))*1 80/pi; 

%  Ensure  correct  LOS  angle  between  initial  vehicle  position  and  cage  position  for  different 
conditions 

% - 

if  ((X(1)-X_Way_c(1))<0)  &  ((Y(1)-Y_Way_c(1))<0) 
theta=180+theta; 
end 

if  ((X(1)-X_Way_c(1))>0)  &  ((Y(1)-Y_Way_c(1))>0) 
theta=theta; 
end 

if  ((X(1)-X_Way_c(1))<0)  &  ((Y(1)-Y_Way_c(1))>0) 
theta=180+theta; 
end 

if  ((X(1)-X_Way_c(1))>0)  &  ((Y(1)-Y_Way_c(1))<0) 
theta=360+theta; 
end 

if  ((X(1  )-X_Way_c(1  ))==0)  &  ((Y(1  )-Y_Way_c(1  ))<0) 
theta=360+theta; 
end 

if  ((X(1  )-X_Way_c(1  ))==0)  &  ((Y(1  )-Y_Way_c(1  ))>0) 
theta=360+theta; 
end 

if  ((X(1)-X_Way_c(1))>0)  &  ((Y(1)-Y_Way_c(1))==0) 
theta=360+theta; 
end 

if  ((X(1)-X_Way_c(1))<0)  &  ((Y(1)-Y_Way_c(1))==0) 
theta=180+theta; 
end 

% - 

%LOS  distance  from  initial  vehicle  position  to  cage  position 
Length  =  sqrt((X_Way_c(1)-X(1))''2+(Y_Way_c(1)-Y(1))''2); 
initial_Length  =  Length; 

%  To  determine  the  cage  orientation 

%X  co-ordinate  of  the  initial  vehicle  position  from  the  cage  position  -  for  plotting 
X_LOS  =  X_Way_c(1)+initial_Length*cos(theta  *DegRad); 

%Y  co-ordinate  of  the  initial  vehicle  position  from  the  cage  position  -  for  plotting 
Y_LOS  =  Y_Way_c(1)+initial_Length*sin(theta  *DegRad); 
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%  X  co-ordinate  of  centre  of  cone  which  is  of  distance  "Length"  away  from  cage  position 
X_cage  =  X_Way_c(1)+initial_Length*cos(initial_cage_psi  *DegRad); 

%  Y  co-ordinate  of  centre  of  cone  which  is  of  distance  "Length"  away  from  cage  position 
Y_cage  =  Y_Way_c(1)+initial_Length*sin(initial_cage_psi  *DegRad); 

%  X  co-ordinate  of  left  limit  of  cone  of  the  cage  (2D) 

L_cone_x  =  X_Way_c(1)+cage_length*cos((initial_cage_psi  +30)*DegRad); 

%  Y  co-ordinate  of  left  limit  of  cone  of  the  cage  (2D) 

L_cone_y  =  Y_Way_c(1)+cage_length*sin((initial_cage_psi  +30)*DegRad); 

%  X  co-ordinate  of  right  limit  of  cone  of  the  cage  (2D) 

R_cone_x  =  X_Way_c(1)+cage_length*cos((initial_cage_psi  -30)*DegRad); 

%  Y  co-ordinate  of  right  limit  of  cone  of  the  cage  (2D) 

R_cone_y  =  Y_Way_c(1)+cage_length*sin((initial_cage_psi  -30)*DegRad); 

%To  allow  plotting  when  part  of  cone  co-ordinate  equals  cage  position  co-ordinates 

% - 

if  ((L_cone_x-X_Way_c(1))  ==  0) 
xjint  =  1 ; 
else 

xjint  =  L_cone_x-X_Way_c(1); 
end 

if  ((L_cone_y-Y_Way_c(1))  ==  0) 
yjint  =  1 ; 
else 

yjint  =  L_cone_y-Y_Way_c(1); 
end 

if  ((R_cone_x-X_Way_c(1))  ==  0) 
x_rint  =  1 ; 
else 

x  rint  =  R_cone_x-X  \A/ay_c(1); 
end 

if  ((R_cone_y-Y_Way_c(1))  ==  0) 
y_rint  =1; 
else 

y_rint  =  R_cone_y-Y_Way_c(1); 
end 

0/„ - 

%  angle  between  LOS  angle  and  cage  initial  orientation  angle 
Diff_angle  =  theta-initial_cage_psi; 

%  DeWrap  Diff_angle  to  within  +/-  pi;  Normalized  to  Lie  between  +/- 180 
%  degrees 

while(abs(Diff_angle)  >  180) 

Diff_angle  =  Diff_angle  -  sign(Diff_angle)*360; 
end; 

%  Compute  the  number  of  waypoints  and  their  positions 
[X  \A/ay_c,Y  \A/ay_c,SurfPhase,W_R, No  tracks] 

waypoint3(X_cage,Y_cage,XcageJnitial,YcageJnitial,Diff_angle,initial_cage_psi, Length) 

%  Total  Track  Length  between  initial  waypoint  and  waypoint  (1) 

SegLen(l)  =  sqrt((X_Way_c(1)-PrevX_Way_c(1))''2+(Y_Way_c(1)... 

-PrevY_Way_c(1  ))''2); 

%  T rack  Angle  of  first  track 

psijrack(1 )  =  atan2(Y_Way_c(1  )-PrevY_Way_c(1  ),X_Way_c(1  )-PrevX_Way_c(1 )); 
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%  Track  Length  and  track  angle  for  subsequent  tracks 
for  j=2:No_tracks, 

SegLenQ)  =  sqrt((X_Way_c(j)-X_Way_c(j-1))''2+(Y_Way_c(j)-... 

Y_Way_cG-1))''2); 

psi_trackG)  =  atan2(Y_Way_cG)-Y_Way_cG-1  ),X_Way_cG)-X_Way_cG-1 )); 
end; 

cage_rate  =2;  %Cage  rotation  rate 

cage_psi  =  initial_cage_psi; 

j=1; 

for  i  =  1:length(t)-1, 

Depth_com(i)  =WayPointVertDist_com; 
time  =  time  +  dt; 

%  Cage  position  updates  to  the  vehicle  at  every  three  seconds 

%Real  cage  orientation  at  any  point  in  time 

real_cage_psi=  60*sin(cage_rate*pi/1 80*time)+initial_cage_psi; 

%  determine  the  direction  of  rotation:  direction=1  ==>  anti-clockwise 
if  (direction  ==  1) 

estimate_cage_psi  =  cage_rate*dt+cage_psi; 
else 

estimate_cage_psi  =  -cage_rate*dt+cage_psi; 
end 

%  sensor  update  from  the  cage  every  3  seconds 

if  ((round(time)-time)<1e-6)  &  ((round(time)-time)>-1e-6)  &  (mod(round(time),3)  ==  0) 
cage_psi  =  reai_cage_psi; 
cage_psi_old; 

cage_rate  =  2  %  cage  rate  measurement  from  cage 

%  determine  the  direction  of  rotation:  directional  ==>  anti-clockwise 
if  (cage_psi>cage_psi_old) 
direction  =  1; 
else 

direction  =  0; 
end 

%cage_psi  =  initial_cage_psi+2*time  %  Initial  cage  yaw  angle 
estimatecagepsi  =  cagepsi; 
cage_psi_old  =  cage_psi; 
end 

%  Set  initial  conditions 

j=i; 

time; 

%  Set  start  position 

X_Way_c(1)  =  Xcagejnitial;  %  Reset  waypoint  #1  as  the  cage  position 

Y_Way_c(1)  =  Ycagejnitial; 

W_R(1)  =  1.0;  %  Sets  initial  Watch  Radius 

SurfPhase(l)  =  0; 

Notracks  =1; 

Depth_com(1)  =  5.0; 

WayPointVertDistcom  =  5.0; 

SURFACE_TIMER_ACTIVE  =  FALSE; 

PrevX_Way_c(1)  =  X(i);  %  Set  initial  waypoint  to  current  vehicle  position 

PrevY_Way_c(1)  =  Y(i); 
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%To  determine  LOS  angle 

%LOS  angle  from  current  position  of  vehicle  to  waypoint  #1(also  cage  position) 
theta  =  atan((Y(i)-Y_Way_c(1))/(X(i)-X_Way_c(1)))*1 80/pi; 

%  Ensure  correct  LOS  angle  between  current  vehicle  position  and  waypoint  #1(also  cage 
position)  for  different  conditions 

% - 

if  ((X(i)-X_Way_c(1))<0)  &  ((Y(i)-Y_Way_c(1))<0) 
theta=180+theta; 
end 

if  ((X(i)-X_Way_c(1))>0)  &  ((Y(i)-Y_Way_c(1))>0) 
theta=theta; 
end 

if  ((X(i)-X_Way_c(1))<0)  &  ((Y(i)-Y_Way_c(1))>0) 
theta=180+theta; 
end 

if  ((X(i)-X_Way_c(1))>0)  &  ((Y(i)-Y_Way_c(1  ))<0) 
theta=360+theta; 
end 

if  ((X(i)-X_Way_c(1))==0)  &  ((Y(i)-Y_Way_c(1))<0) 
theta=360+theta; 
end 

if  ((X(i)-X_Way_c(1))==0)  &  ((Y(i)-Y_Way_c(1))>0) 
theta=360+theta; 
end 

if  ((X(i)-X_Way_c(1))>0)  &  ((Y(i)-Y_Way_c(1  ))==0) 
theta=360+theta; 
end 

if  ((X(i)-X_Way_c(1))<0)  &  ((Y(i)-Y_Way_c(1))==0) 
theta=180+theta; 
end 


%LOS  distance  from  current  vehicle  position  to  cage  position 
Length  =  sqrt((X_Way_c(1)-X(i))''2+(Y_Way_c{1)-Y(i))''2); 

To  determine  the  cage  orientation 

%X  co-ordinate  of  the  current  vehicle  position  from  the  cage  position  -  for  plotting 
X  LOS  =  X_Way_c(1)+Length*cos(theta  *DegRad); 

%Y  co-ordinate  of  the  current  vehicle  position  from  the  cage  position  -  for  plotting 
Y_LOS  =  Y_Way_c(1)+Length*sin(theta  *DegRad); 

X  cage  =  X_Way_c(1)+Length*cos(estimate_cage_psi  *DegRad); 

Y_cage  =  Y_Way_c(1)+Length*sin(estimate_cage_psi  *DegRad); 

%  X  co-ordinate  of  left  limit  of  cone  of  the  cage  (2D) 

L_cone_x  =  X_Way_c(1)+cage_length*cos((real_cage_psi  +30)*DegRad); 

%  Y  co-ordinate  of  left  limit  of  cone  of  the  cage  (2D) 

L_cone_y  =  Y_Way_c(1)+cage_length*sin((real_cage_psi  +30)*DegRad); 

%  X  co-ordinate  of  right  limit  of  cone  of  the  cage  (2D) 

R_cone_x  =  X_Way_c(1)+cage_length*cos((real_cage_psi  -30)*DegRad); 

%  Y  co-ordinate  of  right  limit  of  cone  of  the  cage  (2D) 

R_cone_y  =  Y_Way_c(1)+cage_length*sin((real_cage_psi  -30)*DegRad); 

%To  allow  plotting  when  part  of  cone  co-ordinate  equals  cage  position  co-ordinates 

% - 

if  ((L_cone_x-X_Way_c(1 ))  ==  0) 
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xjint  =  1; 
else 

xjint  =  L_cone_x-X_Way_c(1); 
end 

if  ((L_cone_y-Y_Way_c(1 ))  ==  0) 
yjint  =  1; 
else 

yjint  =  L_cone_y-Y_Way_c(1); 
end 

if  ((R_cone_x-X_Way_c(1))  ==  0) 
x_rint  =  1 ; 
else 

x  rint  =  R_cone_x-X  \A/ay_c(1); 
end 

if  ((R_cone_y-Y_Way_c(1))  ==  0) 
y_rint  =1; 
else 

y_rint  =  R_cone_y-Y_Way_c(1); 
end 


%  angle  between  LOS  angle  and  cage  initial  orientation  angle 
Diff_angle  =  theta-estimate_cage_psi; 

%  DeWrap  Diff_angle  to  within  +/-  pi;  Normalized  to  Lie  between  +/- 180 
%  degrees 

while(abs(Diff_angle)  >  180) 

Diff_angle  =  Diff_angle  -  sign(Diff_angle)*360; 
end; 

%  Compute  the  number  of  waypoints  and  their  positions 

[X  \A/ay_c,Y  \A/ay_c,SurfPhase,W_R, No  tracks]  = 

waypoint3(X_cage,Y_cage,XcageJnitial,YcageJnitial,Diff_angle,estimate_cage_psi, Length); 

%  Total  Track  Length  between  initial  waypoint  and  waypoint  (1) 

SegLen(l)  =  sqrt((X_Way_c(1)-PrevX_Way_c(1))''2+(Y_Way_c(1)... 

-PrevY_Way_c(1  ))''2); 

%  T rack  Angle  of  first  track 

psijrack(1 )  =  atan2(Y_Way_c(1  )-PrevY_Way_c(1  ),X_Way_c(1  )-PrevX_Way_c(1 )); 

%  Track  Length  and  track  angle  for  subsequent  tracks 
for  j=2:No  tracks, 

SegLen(j)  =  sqrt((X_Way_cG)-X_Way_cG-1))''2+(Y_Way_cG)-... 

Y_Way_cG-1))''2); 

psiJrackQ)  =  atan2(Y_Way_cG)-Y_Way_cG-1  ),X_Way_cG)-X_Way_cG-1 )); 
end; 
j=i; 

%  Difference  between  current  vehicle  position  &  the  next  waypoint  Eq(13) 

X_Way_Error(i)  =  X_Way_cG)  -  X(i); 

Y_Way_Error(i)  =  Y_Way_cG)  -  Y(i); 

%  DeWrap  psi  to  within  +/-  2.0*pi;  Makes  Heading  Angle  to  lie  between 
%  0-360  degrees 

psi_cont(i)  =  psi(i); 
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while(abs(psi_cont(i))  >  2.0*pi) 
psi_cont(i)  =  psi_cont(i)  -  sign(psi_cont(i))*2.0*pi; 
end; 

%  Cross  Track  Heading  Error  Eq(12) 

psi_errorCTE(i)  =  psi_cont(i)  -  psi_track(j); 

%  DeWrap  psi_error  to  within  +/-  pi;  Normalized  to  Lie  between  +/- 180 
%  degrees 

while(abs(psi_errorCTE(i))  >  pi) 

psi_errorCTE(i)  =  psi_errorCTE(i)  -  sign(psi_errorCTE(i))*2.0*pi; 
end; 

%  **  Always  Calculate  this  (What  is  This?) 

Beta  =  v(i)/U; 

%  Beta  =  0.0; 

cpsi_e  =  cos(psi_errorCTE(i)+Beta); 
spsi_e  =  sin(psi_errorCTE(i)+Beta); 

%  Distance  to  the  ith  way  point  projected  to  the  track  line  S(t)i  - 
%  Eq  (14) 

s(i)  =  [X_Way_Error(i),Y_Way_Error(i)]*[(X_Way_cG)-... 
PrevX_Way_cG)),(Y_Way_cG)-PrevY_Way_cG))]'; 

%  s  is  distance  to  go  projected  to  track  line(goes  from  0-100%L)  -  Eq  (14) 

s(i)  =  s(i)/SegLenG); 

Ratio=(1 .0-s(i)/SegLenG))*1 00.0;  %  Ranges  from  0-1 00%  of  SegLen 

%  Radial  distance  to  go  to  next  WP 

ss(i)  =  sqrt(X_Way_Error(i)''2  +  Y_Way_Error(i)''2); 

%  dp  is  angle  between  line  of  sight  and  current  track  line  -  Eq  (16) 

dp(i)  =  atan2(  (Y_Way_cG)-PrevY_Way_cG)),(X_Way_cG)-... 

PrevX_Way_cG)) )-  atan2(  Y_Way_Error(i),X_Way_Error(i) ); 
if(dp(i)  >  pi), 

dp(i)  =  dp(i)-2.0*pi; 
end; 

%  Cross  Track  Error  Definition  -  Eq  (15) 
cte(i)  =  s(i)*sin(dp(i)); 

%  If  the  magnitude  of  the  CTE  Heading  exceeds  40  degrees,  a  LOS  Controller 
%  is  used. 

if(  abs(psi_errorCTE(i))  >=  40.0*pi/180.0  |  s(i)  <  0.0  ), 

LOS(i)  =  1; 

psi_comLOS  =  atan2(Y_Way_Error(i),X_Way_Error(i));  %  Eq  (22) 
psi_errorLOS(i)  =  psi_comLOS  -  psi_cont(i);  %  Eq  (23) 

%  LOS  Error 
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if(abs(psi_errorLOS(i))  >  pi), 

psi_errorLOS(i)  =  psi_errorLOS(i)  -  2.0*pi*psi_errorLOS(i)... 
/abs(psi_errorLOS(i)); 

end; 

%  Eq  (8) 

Sigma_FlightHeading  =  0.9499*(r_com  -  r(i))  +  0.170rpsi_errorLOS(i); 
%  Eq  (9) 

dr(i)  =  -1.5435*(  2.5394*r(i)+  Eta_FlightHeading*tanh... 
(Sigma_FlightHeading/Phi_FlightHeading)); 

else 

%  Use  CTE  Controller  if  GTE  Heading  is  less  than  40  degrees 
LOS(i)  =  0; 

if(cpsi_e  ~=  0.0),  %  Trap  Div.  by  Zero  ! 

%  SMC  Soln 

%  Sliding  Surface  -  Eq  (20) 

Sigma(i)  =  U*rRM(i)*cpsi_e  +  Lam1*U*spsi_e  +  3.28*Lam2*cte(i); 

%  Rudder  Input -Eq  (21) 

dr(i)  =  (1.0/(U*b*cpsi_e))*(-U*a*rRM(i)*cpsi_e  +  U*rRM(i)''2*... 
spsi_e  -  Lam1*U*rRM(i)*cpsi_e  -  Lam2*U*spsi_e  -  Eta_CTE* ... 
(Sigma(i)/Phi_CTE)); 

else 

dr(i)  =  dr(i-1); 
end; 

end;  %  End  of  CTE  Controller 


%  end; 

if(abs(dr(i))  >  0.4) 
dr(i)  =  0.4*sign(dr(i)); 
end; 


x_dot(:,i+1)  =  [  A(1,1)*v(i)  +  A(1,2)*r(i)  +  B(1)*dr(i); 
A(2,1)*v(i)  +  A(2,2)*r(i)  +  B(2)*dr(i); 
r(i)]; 

x(:,i+1)  =  x(:,i)+dt*x_dot(:,i); 
v(i+1)  =x(1,i+1)/12; 
r(i+1)  =x(2,i+1); 
psi(i+1)  =  x(3,i+1); 
rRM(i+1)  =  r(i+1); 
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Uc  =  0.0; 

Vc  =  0.0; 

%  Kinematics 

X(i+1)  =  X(i)  +  (Uc  +  (U/3.28)*cos(psi(i))  -  v(i)/3.28*sin(psi(i))  )*dt; 

Y(i+1)  =  Y(i)  +  (Vc  +  (U/3.28)*sin(psi(i))  +  v(i)/3.28*cos(psi(i))  )*dt; 

************************************************************************** 

%  Check  if  Aries  falls  within  the  cage 

X_tip(i)  =  X(i)  +  xcg/3.28*cos(psi(i));  %  X  co-ordinate  of  tip  of  the  vehicle's  nose 

Y_tip(i)  =  Y(i)  +  xcg/3.28*sin(psi(i));  %  Y  co-ordinate  of  tip  of  the  vehicle's  nose 

%FOV  extended  to  the  initial  vehicle  start  point's  distance 

Xcurrent  =  X_tip(i); 

Ycurrent  =  Y_tip(i); 
psicurrent  =  psi(i); 

%Angle  of  vehicle  tip  w.r.t  cage  position 

vehicle_cage_angle  =  atan2(Y_tip(i)-Ycage_initial,X_tip(i)-Xcage_initial)*1 80/pi; 

%  DeWrap  vehicle_cage_angie  to  within  +/-  pi;  Normalized  to  Lie  between  +/- 180 
%  degrees 

while(abs(vehicle_cage_angle)  >  180) 

vehicle_cage_angle  =  vehicie_cage_angle  -  sign(vehicle_cage_angle)*360; 
end; 

vehicle_cage_angle  =  mod(vehicle_cage_angle,360); 


Delta_angle  =  vehicle_cage_angle-real_cage_psi;  %in  degree 
LengthLOS  =  sqrt((Y_tip(i)-Ycage_initial)''2+(X_tip(i)-Xcage_initial)''2); 

LengthLOSreal  =  sqrt((Y(i)-Ycage_initial)''2+(X(i)-Xcage_initial)''2); 

maxdistance  =  cage_length*cos(30*DegRad); 
currentdistance  =  LengthLOS*cos(Delta_angle*DegRad); 
currentdistancereal  =  LengthLOSrearcos(Delta_angle*DegRad); 
mindistance  =  cage_length*cos(30*DegRad)-0.3125; 

minangle  =  mod(real_cage_psi-30,360); 
vehicle_cage_angle; 

maxangle  =  mod(real_cage_psi+30,360); 
num=num+1; 

vca(num)  =vehicle_cage_angle; 
timel(num)  =  time; 
psil(num)  =  psi(i); 
delta(num)  =  Delta_angle; 
diff(num)  =Diff_angle; 

%Set  acceptance  criteria  tolerances  -0.6ft  from  cage  cone  length 

L_cone_xmin  =  Xcage_initial+(cage_length-(0.3125/cos(30*DegRad)))*cos(mod(real_cage_psi 
+30,360)*DegRad); 

L_cone_ymin  =  Ycage_initlal+(cage_length-(0.3125/cos(30*DegRad)))*sin(mod(real_cage_psi 
+30,360)*DegRad); 


80 


R_cone_xmin  =  Xcage_initial+(cage_length-(0.3125/cos(30*DegRad)))*cos(mod(real_cage_psi 
-30,360)*DegRad); 

R_cone_ymin  =  Ycage_initial+(cage_length-(0.3125/cos(30*DegRad)))*sin(mod(real_cage_psi 
-30,360)*DegRad); 

status  =0; 

if  (mod(real_cage_psi,360)  <  30)|  (mod(real_cage_psi,360)  >330) 

if  (vehicle_cage_angle>=mod(real_cage_psi-30,360))  | 

(vehicle_cage_angle<=mod(real_cage_psi+30,360)) 
disp(sprintf('Vehicle  is  within  the  FOV  of  cage')); 
status  =1; 
end 

end 

if  (vehicle_cage_angle>=mod(real_cage_psi-30,360))  & 

(vehicle_cage_angie<=mod(real_cage_psi+30,360)) 
disp(sprintf('Vehicle  is  within  the  FOV  of  cage')); 
status  =1; 

end 

if  (status  ==  1) 

Rconex; 

Rconexmin; 

if  (R  cone  x  ==  L  cone  x) 

if  (abs(vehicle_cage_angle)  <=  90)|(abs(vehicle_cage_angle)  >=  270) 
if  (X_tip(i)  <=  L_cone_x)  &  (X_tip(i)  >=  L_cone_xmin) 
disp(sprintf('Vehicle  is  about  to  dock  into  the  cage')); 
if  ((Y_tip(i)  >  L_cone_y)  |  (Y_tip(i)  <  R_cone_y)) 
clisp(sprintf('Vehicle  misses  the  cage')); 
else 

disp(sprintf('Vehicle  docks  into  the  cage')); 
break; 
end 
end 
end 

if  (abs(vehicle_cage_angle)  >  90)&(abs(vehicle_cage_angle)  <  270) 
if  (X_tip(i)  >=  L_cone_x)  &  (X_tip(i)  <=  L_cone_xmin) 
disp(sprintf('Vehicle  is  about  to  dock  into  the  cage')); 
if  ((Y_tip(i)  >  R_cone_y)|(Y_tip(i)  <  L_cone_y)) 
clisp(sprintf('Vehicle  misses  the  cage')); 
else 

disp(sprintf('Vehicle  docks  into  the  cage')); 
break; 
end 
end 
end 

end 

if  (R  cone  x  ~=  L  cone  x) 

if  (LengthLOS*cos(Delta_angle*DegRad)  <=  cage_length*cos(30*DegRad))  &  ... 
(LengthLOS*cos(Delta_angle*DegRad)  >  cage_length*cos(30*DegRad)-0.3125) 
disp(sprintf('Vehicle  is  about  to  dock  into  the  cage')); 
disp(sprintf('Vehicle  docks  into  the  cage')); 
break; 
end 
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end 

else 

disp(sprintf('Vehicle  is  NOT  within  the  FOV  of  cage')); 
end 

status  =0; 

%  Check  to  See  if  we  are  Within  the  Watch_Radius 
test  =  j; 

test1  =No_tracks; 

test2  =  sqrt(X_Way_Error(i)''2.0  +  Y_Way_Error(i)''2.0); 
tests  =X_Way_Error(i); 
test4  =  Y_Way_Error(i); 
tests  =  s(i); 

if(sqrt(X_Way_Error(i)''2.0  +  Y_Way_Error(i)''2.0)  <=  W_RG)  |  s(i)  <  0.0) 
disp(sprintf('WayPoint  %d  Reached',))); 
ifG==No_tracks) 

X_Way_Error(i); 

Y_Way_Error(i); 

break; 

end; 

PrevX_Way_cG+1)  =  X_Way_cG); 

PrevY_Way_cG+1)  =  Y_Way_cG); 

Fj+1; 

end; 

cage_psi=estimate_cage_psi; 

end; 

plot(X_LOS,Y_LOS,'s') 
hold  on 
plot(X,Y); 
hold  on 

plot(Xcage_initial:x_rint:R_cone_x,Ycage_initial:y_rint:R_cone_y,'r') 
hold  on 

plot(Xcage_initial:x_lint:L_cone_x,Ycage_initial:y_lint:L_cone_y,'r') 
hold  on 

plot([X_tip(i)],[Y_tip(i)],'k.'); 
hold  on 

if  (Xcage_initial>  X(1 ))  &  (Ycage_initial>  Y(1 )) 

Axis([X(1)-100  Xcage_initial+100  Y(1)-100  Ycage_initial+100]); 
end 

if  (Xcage_initial<  X(1 ))  &  (Ycage_initial<  Y(1 )) 

Axis([Xcage_initial-1 00  X(1 )+ 1 00  Ycage_initial-1 00  Y(1 )+ 1 00]); 
end 

if  (Xcage_initial<  X(1))  &  (Ycage_initial>  Y(1)) 

Axis([Xcage_initial-1 00  X(1 )+ 1 00  Y(1  )-1 00  Ycage_initial+1 00]); 
end 

if  (Xcage_initial>  X(1 ))  &  (Ycage_initial<  Y(1 )) 

Axis([X(1)-100  Xcage_initial+100  Ycage_initial-100  Y(1)+100]); 

end 
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